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PROJECT  OVERVIEW 

Many  virtual  environment  (VE)  applications,  such  as  a  system  for  training  team 
members  to  perform  vehicle  maintenance,  call  for  the  representation  and  display  of 
interactive,  realtime  synthetic  humans,  which  we  call  virtual  actors.  In  order  to  do  this, 
we  need  to  develop  adaptive  and  efficient  computational  models  of  human  motor 
behavior. 

In  the  past  year,  we  have  undertaken  a  research  program  designed  to  identify  the 
basic  issues  involved  in  developing  a  system,  which  we  call  SkillBuilder,  for  computer- 
aided  design  of  motor  skills  for  virtual  actors.  The  skill  builder  will  allow  us  to 
implement  a  generic  human  model,  and  “train”  it  to  adaptively  execute  new  skills,  such 
as  handling  and  operating  a  particular  tool.  As  part  of  this  effort,  we  have  implemented  a 
model  of  visually-guided  reaching  and  grasping,  and  controllers  for  human  head  and  eye 
movement  based  on  psychophysical  studies.  We  have  demonstrated  that  these  models  can 
support  realistic,  realtime  interaction  with  human  VE  participants.  This  work  is 
described  in  detail  in  [2, 10] . 

Realtime,  interactive  behavior  models  would  be  of  little  use  without  appropriate 
human  interface  software  that  supports  task  level  interaction  using  speech  and  gestures. 
In  addition  to  our  work  on  SkillBuilder,  therefore,  we  have  also  developed  an  architecture 
that  we  believe  will  support  task  level  interaction  using  a  restricted  natural  language 
interface.  This  work  is  based  on  our  previous  work  on  reactive  planning  systems  [1 1],  as 
well  as  the  natural  language  research  of  Schank  and  colleagues  in  the  early  70s  [6,  7], 
efforts  to  develop  a  language  for  specifying  human  behavior  in  a  clinical  setting  [5,  8], 
and  the  notion  of  affordances  first  described  by  J.  J.  Gibson  [3,4]. 

Finally,  in  addition  to  modeling  human  motor  behavior  for  virtual  actors,  it  is 
important  to  correctly  capture  human  appearance,  especially  when  close-up  interactions 
are  required.  Therefore,  we  have  also  continued  our  efforts  to  develop  accurate,  realtime 
finite  element  models  (FEM)  of  human  facial  tissue.  This  year  we  have  developed  a  set 
of  motor  programs  that  coordinate  the  actions  of  our  finite  element  model  of  the  human 
face,  such  that  we  can  display  a  range  of  facial  expressions.  Since  we  cannot  update  the 
FEM  in  realtime,  we  have  also  developed  a  method  for  pre-computing  a  set  of  “key 
expressions,”  and  later  playing  them  back  in  realtime.  This  is  also  shown  on  the 
accompanying  videotape. 
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REALTIME,  MULTI-MODAL  INTERACTION  WITH  VIRTUAL  ACTORS 

In  August  1992  we  set  out  to  implement  a  virtual  environment  that  includes 
autonomous  and  interactive  synthetic  humans.  These  virtual  actors  should  be  capable  of 
interacting  with  human  participants  in  the  VE,  as  well  as  with  simulated  objects  and 
events  in  the  VE.”  [ —  From  the  proposal  to  ONR] 

On  July  30,  1993,  we  were  able  to  demonstrate  a  prototypical  virtual  actor  capable  of 
responding,  in  realtime,  to  physical  interaction  with  human  participants  using  position¬ 
sensing  devices.  In  addition,  this  virtual  actor  also  performed  interactive,  realtime, 
visually-guided  grasping  and  reaching  behaviors.  Finally,  the  actor  was  capable  of 
responsive  head  and  eye  movements.  Not  only  could  it  track  target  objects  with  realistic 
head  and  eye  movements,  but  if  a  human  were  to  wear  a  head-mounted  position-sensor, 
the  actor  would  maintain  eye  contact  with  the  human  as  he  or  she  moved!  This  work  is 
reported  in  [1, 9].  Please  also  see  the  videotape  that  accompanies  this  report. 

While  there  are  a  small  number  of  human-modeling  efforts  underway  elsewhere  in  the 
U.S.  and  Europe,  our  work  is  unique  because  of  our  focus  on  realtime,  adaptive,  multi¬ 
modal  interaction,  rather  than  offline  scripting  of  behaviors. 

TASK  LEVEL  INTERACTION  WITH  VIRTUAL  ACTORS 

In  earlier  work  we  developed  and  implemented  a  reactive  planner,  which  we  call  the 
skill  network,  for  adaptively  selecting  and  executing  behavior  modules,  called  motor 
skills,  in  order  to  satisfy  some  motor  goal,  say,  “Go  to  the  door  and  open  it.”  In  order  to 
do  this,  however,  we  need  to  identify  the  motor  skills  explicitly  and  implicitly  required  to 
accomplish  the  given  motor  act.  We  call  this  process  “motor  goal  parsing,”  (MGP)  and 
this  year  we  have  designed  an  architecture  we  feel  will  support  this  process. 

While  most  humans  are  capable  of  seemingly  effortless  manipulation  of  ordinary 
objects  in  everyday  life,  computational  models  of  such  behavior  must  account  for 
affordances  of  objects  and  motor  equivalence.  That  is,  given  a  command  that  names  one 
or  more  objects  in  the  virtual  world,  how  can  a  virtual  actor  compute  at  runtime  whether 
and  how  the  named  objects  can  be  manipulated?  Next,  once  the  virtual  actor  has 
determined  that  an  object  can  be  manipulated,  the  appropriate  effector  system  must  be 
selected.  For  example,  can  the  object  be  grasped  with  one  hand,  or  must  both  arms  be 
employed? 

In  the  past  year,  we  have  described  the  representations  we  feel  are  necessary  to 
transform  descriptions  of  everyday  activities  into  a  series  of  motor  goals  that  can  be 
executed  by  the  skill  network.  In  order  to  account  for  motor  equivalence  and  the 
affordances  of  objects  in  the  environment,  we  have  specified  an  effector-free  set  of  task 
primitives  to  be  used  as  an  intermediate  representation  in  the  parsing  process,  and  we 
have  outlined  the  process  of  motor  goal  parsing.  This  is  described  in  detail  in  [12].  It  is 
our  intention  to  implement  this  architecture  in  the  coming  year. 

PUBLICATIONS 

•  “SkillBuilder:  Designing  Motor  Skills  for  Virtual  Actors’”  D.  Zeltzer  and  S.  Gaffron, 
January  1994,  submitted  for  publication. 

•  SkillBuilder:  A  Motor  Program  Design  Tool  for  Virtual  Actors,  S.  Gaffron,  M.S. 
Thesis,  February  1994,  Dept,  of  Mechanical  Engineering,  Massachusetts  Institute  of 
Technology,  Cambridge  MA. 

•  “Control  and  Coordination  of  Head,  Eyes  and  Facial  Expressions  of  Virtual  Actors  in 
Virtual  Environments,”  S.  K.  Singh,  S.  D.  Pieper,  2nd  IEEE  International  Workshop  on 
Robot  and  Human  Communication,  Tokyo,  November  3-5,  1993. 
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•  “The  Virtual  Sailor:  An  Implementation  of  Interactive  Human  Body  Modeling,”  D.  T. 
Chen,  S.  D.  Pieper,  S.  K.  Singh,  J.  M.  Rosen,  D.  Zeltzer,  Proc.  1993  Virtual  Reality 
Annual  International  Symposium,  Seattle,  WA,  September,  1993,  pp.  429-435. 

•  “Virtual  Actors  and  Virtual  Environments:  Defining,  Modeling  and  Reasoning  about 
Motor  Skills,”  D.  Zeltzer,  M.  B.  Johnson,  in  Interacting  with  Virtual  Environments, 
L.  MacDonald  and  J.  Vince,  eds.,  John  Wiley  and  Sons,  in  press.  Reprinted,  with 
revisions,  from  Proc.  British  Computer  Society  International  Conf.  on  Interacting  with 
Images,  London,  February  10-11, 1993. 
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ABSTRACT 

In  many  virtual  environment  (VE)  applications,  e.g.,  a  VE 
system  for  training  personnel  to  perform  multi-person 
maintenance  tasks,  the  VE  system  must  be  able  to  display 
accurate  models  of  human  figures  that  can  perform  routine 
behaviors  and  adapt  to  events  in  the  virtual  world.  In  order 
to  achieve  such  adaptive,  task  level  interaction  with  virtual 
actors,  it  is  necessary  to  model  elementary  human  motor 
skills.  SkillBuilder  is  a  software  system  for  constructing  a 
set  of  motor  behaviors  for  a  virtual  actor  by  designing 
motor  programs  for  arbitrarily  complicated  skills.  Motor 
programs  are  modeled  using  finite  state  machines,  and  we 
have  categorized  a  set  of  transition  and  ending  conditions. 
Using  inverse  kinematics  and  automatic  collision 
avoidance,  we  use  SkillBuilder  to  construct  a  suite  of 
behaviors  for  simulating  visually  guided  reaching, 
grasping,  and  head/eye  traclang  motions  for  a  kinematically 
simulated  actor  consisting  of  rigid  body  parts.  All  of  these 
actions  have  been  successfully  demonstrated  in  real-time  by 
permitting  the  user  to  interact  with  the  virtual  environment 
using  a  whole-hand  input  device. 


1.  VIRTUAL  ACTORS  AND  VIRTUAL  ENVIRONMENTS 

For  more  than  20  years,  the  representation  and  control  of  synthetic  human  figures  has 
been  an  active  area  of  research  in  the  computer  graphics  community  (e.g.,  [3,  12,  19,  37, 
41]).  This  research  is  necessarily  multi-disciplinary,  and  has  involved  major  efforts  in 
four  broad  areas:  motion  control  and  behavior  modeling,  representation  and  control  of 
facial  expressions,  simulation  of  articulated  rigid  body  motion,  and  simulation  and 
display  of  clothing  and  soft  tissue.  While  significant  progress  has  been  made  in  each  of 
these  areas,  much  remains  to  be  done. 


This  work  was  supported  in  part  by  Office  of  Naval  Research  Grant  #N00014-92-J-4005,  NHK  (Japan 
Broadcasting  Corp),  Arpa/Rome  Labs,  and  equipment  gifts  from  Hewlett-Packard  and  Silicon  Graphics. 

Submitted  for  publication  to  ACM  Siggraph  ‘94. 
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Certain  systems,  in  particular,  Jack™  from  the  University  of  Pennsylvania,  and  the 
Human  Factory  system  developed  by  the  Thalmanns,  are  capable  of  presenting  pre¬ 
scripted  scenarios  portraying  human  figures  as  articulated  rigid  bodies  acting  out  pre¬ 
defined  series  of  simple  motor  acts  such  as  walking  and  pushing  buttons  on  a  control 
panel  [2,  23].  These  figures  can  be  positioned  and  manipulated  interactively  during  the 
scripting  process,  but  once  the  scripting  is  complete,  animated  sequences  of  the  human 
figures  are  generated  off-line  and  later  presented  either  on  video  tape  or  on  a  workstation 
monitor.  One  does  not  interact  directly  with  these  figures  for  the  most  part,  except  during 
the  scripting  process,  which  is  akin  to  manipulating  electronic  mannikins. 

The  long-term  goal  of  the  research  described  here  is  the  implementation  of  a  virtual 
environment  (VE)  that  includes  autonomous  and  interactive  synthetic  humans.  We  will 
call  a  computer  model  of  a  human  figure  that  can  move  and  function  in  a  VE  a  virtual 
actor.  If  the  movement  of  a  virtual  actor  is  slaved  to  the  motions  of  a  human  VE 
participant,  we  will  call  that  a  guided  virtual  actor,  or  simply,  a  guided  actor.  Autonomous 
actors  operate  under  program  control  and  are  capable  of  independent  and  adaptive 
behavior.  Autonomous  actors  should  be  capable  of  interacting  —  in  realtime  —  with 
simulated  objects  and  events  in  the  VE,  as  well  as  with  other  virtual  actors  and  human 
participants  in  the  VE.  Among  other  applications,  the  inclusion  of  virtual  actors  in 
simulation  and  .training  systems  will  enable  training  and  rehearsal  of  operations  which 
require  multiple  team  members,  e.g.,  carrier  flight  deck  operations  and  shipboard 

firefighting.  .  ,  , 

Every  virtual  actor  in  a  VE  must  have  a  set  of  behaviors  it  can  perform,  whether  the 
actor  represents  a  robot  capable  of  only  a  few  simple  actions,  or  whether  the  actor  is  a 
synthetic  human  possessing  a  wide  range  of  adaptive  motor  skills.  Conventional 
animation  techniques,  such  as  key  framing,  however,  are  wholly  inadequate  for  realtime 
interaction  with  the  end-users  of  VE  systems.  Such  users  need  to  interact  with  virtual 
actors  in  realtime  at  the  task  level,  through  language  and  gesture,  in  ways  familiar  from 
everyday  life.  Our  focus  therefore  is  on  modeling  elementary  human  motor  skills,  and 
integrating  these  skills  into  a  repertoire  of  behaviors  for  an  autonomous  actor. 

The  runtime  mechanism  for  selecting  and  sequencing  motor  skills  to  generate  the 
routine  behaviors  of  a  virtual  actor  must  link  perception  with  action,  in  a  process  we  call 
motor  planning.  In  earlier  work  we  have  described  the  architecture  for  a  skill  network  we 
have  implemented  to  model  the  routine  behaviors  of  virtual  actors,  and  we  have  also 
characterized  the  kinds  of  behaviors  the  skill  network  can  capture,  and  the  sorts  of 
behavior  it  cannot  model  [43].  For  each  virtual  actor  we  need  to  construct  a  skill  network 
which  will  enable  it  to  respond  to  simulated  events  and  human  input  adaptively,  and  in 
real  time.  Each  node  in  the  skill  network  is  a  motor  skill,  such  as  walking,  standing  up, 
reaching  and  grasping  objects,  and  so  on.  SkillBuilder  is  a  prototype  software  system  that 
serves  as  a  CAD  tool  for  constructing  motor  programs,  the  constituent  modules  for  the 
motor  skills  a  virtual  actor  can  perform  [34,  39,  41].  Motor  programs  themselves  may  be 
composed  of  a  set  of  simpler  local  motor  programs. 

The  objective  of  this  work  was  to  develop  a  set  motor  skills  to  model  an  actor’s 
behavior,  and  to  understand  what  is  needed  to  build  a  visual  programming  environment 
for  defining  motor  skills.  Finite  state  machines  have  been  chosen  to  represent 
motor  skills,  and  in  order  to  allow  the  user  to  build  a  finite  state  machine,  it  is  essential  to 
characterize  the  necessary  components.  First,  the  desired  action  of  each  state  has  to  be 
identified  by  assigning  appropriate  local  motor  programs  and  second,  initial  conditions, 
as  well  as  transition  conditions  between  the  states  have  to  be  defined.  Furthermore,  it  is 
necessary  to  deal  with  issues  like  inverse  kinematics  and  collision  avoidance. 

Using  SkillBuilder,  we  have  constructed  a  suite  of  motor  skills  for  simulating  visually 
guided  reaching  and  grasping  —  including  collision  avoidance  and  accompanying 
tracking  head  and  eye  movements  —  for  a  prototypical  virtual  actor  we  call  “Dexter.” 
These  adaptive  reaching  and  grasping  skills  allow  realtime  interaction  between  a  human 
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VE  participant  and  Dexter.  For  example,  a  glass  positioned  on  a  table  located  right  in 
front  of  the  virtual  actor  can  be  ’’picked  up”  and  positioned  anywhere  else  using  a 
Dataglove.  When  Dexter  is  “asked”  to  pick  up  the  glass,  an  appropriate  collision-free 
path  is  generated,  and  Dexter  reaches  for  the  glass,  forming  the  hand  into  the  proper 
grasping  configuration  while  reaching.  Once  the  glass  has  been  reached  the  actor  wraps 
the  fingers  around  the  glass  until  a  contact  has  been  detected  between  each  of  the  fingers 
and  the  glass.  At  any  time  the  glass  can  be  grasped  by  the  human  user  and  repositioned. 
If  Dexter  is  told  to  hang  on  to  it,  appropriate  hand  and  arm  movements  will  be  generated 
so  that  the  glass  remains  upright  in  Dexter’s  grasp.  Otherwise,  Dexter  can  be  asked  to 
release  the  object,  and  pick  it  up  again  anywhere  on  the  table  top  within  reach.  During  all 
of  these  actions,  Dexter  will  track  the  glass  with  correct  head  and  eye  movements.  Please 
see  Figure  1,  as  well  as  the  accompanying  videotape,  which  is  a  recording  of  such  a 
realtime  demonstration. 

The  main  contributions  of  this  work  include  the  generalization  of  the  design  of  motor 
programs  and  the  characterization  of  transition  conditions.  A  motor  program  library  for 
interactive,  visually  guided  reaching  and  grasping  skills  has  been  created.  We  have 
demonstrated  that  this  approach  is  capable  of  generating  approximate  human  movements 
that  look  reasonably  realistic.  The  validation  of  these  movements  with  experimental  data 
is  left  for  future  work. 

In  the  following  section,  we  review  related  work.  In  Section  3,  we  describe  the 
overall  architecture  of  SkillBuilder.  In  section  4,  we  describe  the  implementation  of  the 
major  components  of  SkillBuilder,  including  the  implementation  of  motor  programs  as 
finite  state  machines,  the  categorization  of  transition  procedures,  our  inverse  kinematics 
and  collision  avoidance  routines.  In  Section  5  we  briefly  describe  some  of  the  motor 
skills  we  have  implemented  for  simulating  visually  guided  reaching  and  grasping. 
Finally,  Sections  6  and  7  close  with  discussions  of  future  work,  and  concluding  remarks. 

2.  RELATED  WORK 

Researchers  in  motor  behavior  have  studied  the  underlying  processes  of  motor  control 
in  humans,  and  have  come  to  the  conclusion  that  a  sequence  of  movements  can  be 
coordinated  in  advance  of  their  execution  to  form  a  single  complex  action  [5,  32,  33]. 
They  call  this  process  motor  planning  or  motor  programming.  Bizzi  studied 
visually  triggered  head  and  arm  movements  in  monkeys  under  the  assumption  that  the 
motor  commands  controlling  arm  movement  are  precomputed  somewhere  in  the  central 
nervous  system  prior  to  movement  initiation,  and  that  muscles  can  be  modeled  by  a 
mass/spring  system  including  damping  [5] .  He  found  that  the  motor  program  specifies  an 
intended  equilibrium  point  between  sets  of  agonist  and  antagonist  muscles  that 
correctly  positions  the  arm  and  the  head  in  relation  to  a  virtual  target.  While  we  have  used 
this  control  technique  in  earlier  work  [25],  for  the  time  being,  we  implement  joint 
rotations  with  simple  linear  actuators  in  SkillBuilder,  since  we  are  more  concerned  with 
representing  and  coordinating  motor  skills,  rather  than  the  details  of  joint  motion. 

Schmidt  uses  the  term  motor  program  as  an  abstract  memory  structure  that 
is  prepared  in  advance  of  the  movement  causing  muscle  contractions  and  relaxations 
when  executing  [33].  Furthermore,  he  compares  the  feedback  during  the  execution  of 
motor  programs  to  if-statements  in  computer  programs.  This  suggests  that  using  finite 
state  machines  is  a  good  choice  to  model  motor  programs  in  SkillBuilder. 

Denavit-Hartenberg  joint  notation  is  used  in  SkillBuilder  to  model  the 
kinematic  chains  that  make  up  our  virtual  actor  [15].  This  is  well-described  in  the 
robotics  literature,  e.g.,  [27]. 

Girard  argues  that  the  torque  functions  needed  at  each  joint  to  produce  a  desired  limb 
motion  are  sufficient  to  capture  uncoordinated  limb  motion,  but  that  goal-oriented 
behavior  cannot  be  properly  simulated  without  kinematic  trajectory  planning  [18].  He  has 
been  working  on  inverse  kinematics  and  visual  programming  interfaces  to  describe  motor 
behavior,  and  has  concentrated  mostly  on  locomotion  and  dance.  His  figures  can  be 
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Figure  1.  The  virtual  actor  "Dexter". 
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controlled  interactively  by  specifying  a  spline  curve  as  a  path  for  an  end  effector,  and 
applying  inverse  kinematics  to  compute  the  desired  joint  angles  of  a  limb.  SkillBuilder 
incorporates  all  of  the  methods  he  describes,  in  its  inverse  and  forward  kinematics  skills. 

Investigations  of  knowledge-based  human  grasping  have  been  done  by  Rijpkema  and 
Girard  [31].  Their  hand  model  is  kinematically  similar  to  the  hand  model  used  in 
SkillBuilder.  The  high  level  control  they  propose  for  the  hand  includes  single-finger 
control,  group  control  (closing  and  opening  or  spreading  a  group  of  fingers),  and  hand 
control  (using  a  hand  posture  library).  All  the  named  methods  are  incorporated  in 
SkillBuilder. 

Calvert  et  al  have  developed  a  human  figure  animation  system,  COMPOSE,  that 
provides  for  interactive  key  framing  on  desktop  machines  [11].  Their  system  provides  for 
the  integration  of  independently  developed,  procedural  models  of  motor  skills,  such  as 
walking,  but  does  not  support  on-line  development  of  motor  skills. 

As  discussed  above,  Jack™  largely  acts  out  pre-defined  scripts.  Esakov  and  Badler 

have  described  the  architecture  of  the  Jack™  simulation/animation  system  that  can  handle 
rule  sets,  resource  allocation,  and  temporal  constraints  for  task  sequencing  [16].  No  on¬ 
line  planning  was  implemented,  however.  Task  descriptions  were  initially  in  the  form  of 
pre-defined  “animation  task  keywords”.  This  keyword-based  input  constraint  was 
subsequently  relaxed  to  allow  simple  “do-this-do-that”  commands,  e.g.,  “The  man 
should  flip  [switch]  tglJ-1  with  his  left  hand  and  the  woman  should  move  [switch]  twF-1 
to  position  1.”  Recent  work  by  Badler  and  Webber  involves  the  exploration  of  natural 
language  as  a  means  of  communicating  task  descriptions  [4]. 

Thalmann  and  Magnenat-Thalmann  have  reported  some  work  with  automated 
walking  and  grasping  behaviors,  but  while  the  Human  Factory  has  produced  some  quite 
impressive  human  figure  animation,  their  system  is  based  on  key  frame-like  animation 
for  making  animated  movies,  rather  than  for  realtime  interaction  with  virtual  actors  [22, 
23]. 

Maiocchi  and  Pernici  describe  the  PINOCCHIO  system,  which  is  capable  of  animating 
realistic  human  movement.  However,  their  system  relies  primarily  on  pre-recorded 
human  movements  and  uses  this  data  to  control  the  motion  of  animated  characters  [24]- 

Ridsdale,  et  al,  describe  a  Director's  Apprentice  intended  to  interpret  film  scripts  by 
using  a  rule-base  of  facts  and  relations  about  cinematic  directing  [30].  This  work  was 
primarily  concerned  with  positioning  characters  in  relation  to  the  synthetic  camera,  and 
did  not  address  the  representation  and  control  of  autonomous  agents. 

Brooks  has  developed  and  implemented  a  motor  planning  mechanism  he  calls  the 
subsumption  architecture  [8,  10].  This  work  is  in  large  part  a  reaction  against 
conventional  notions  of  planning  in  Al.  Brooks  argues  for  a  representation-less  paradigm 
in  which  the  behavior  of  a  robot  is  modulated  entirely  by  interaction  between  perception 
of  the  physical  environment  and  the  robot's  “task-achieving  behavior”  modules.  This 
work  is  similar  in  spirit  to  our  own.  Brooks  has  constructed  legged  robots  controlled 
by  networks  of  augmented  finite  state  machines  [9].  He  describes  a  graphical  language 
for  defining  finite  state  machines,  and  this  could  serve  as  the  basis  of  a  visual 
programming  interface  for  defining  and  interconnecting  motor  skills,  in  an  extension  to 
SkillBuilder. 

Agre  has  written  extensively  on  representing  everyday  activities,  and  much  of  his 
work  is  similar  in  spirit  to  our  own  [1].  Unlike  our  work,  however,  Agre  focuses  almost 
exclusively  on  planning  mechanisms  rather  than  on  coordination  and  control  of  the 
kinematic  and  dynamic  simulations  of  motor  skills. 

A  number  of  behavior  modeling  systems  have  been  developed  based  on  either  a 
dataflow  programming  paradigm  or  the  closely  related  cybernetic  wiring  diagram 
method.  The  former  is  exemplified  by  VPL’s  Body  Electric  system  [6],  and  the  latter  by 
Braitenberg’s  Vehicles  [7].  Similar  systems  have  been  reported  by  Travers  [36]  and 
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Wilhelms  [38].  In  our  view,  these  techniques  are  particularly  useful  for  representing  and 
editing  behavior  control  models. 

The  problem  with  each  of  these  systems  is  that  they  have  always  been  used  as  a  sole, 
general-purpose  behavior  modeler  and  controller.  Many  functions,  such  as  physically 
based  control  of  kinematic  linkages,  are  difficult  to  represent  this  way.  However,  as  we 
have  argued  elsewhere,  dataflow  or  cybernetic  diagramming  systems  can  be  productively 
used  to  model  a  range  of  behaviors,  and,  in  addition,  they  are  useful  for  integrating  and 
coordinating  the  action  of  other  kinds  of  behavior  modelers  [42].  We  intend  to  make  use 
of  such  visual  programming  techniques  in  extensions  to  SkillBuilder. 

In  addition  to  SkillBuilder,  we  have  developed  a  software  system  called  WavesWorld 
for  designing,  building  and  debugging  virtual  environments  and  virtual  actors.  A  detailed 
description  can  be  found  in  [44].  A  reactive  planning  algorithm  called  the  motor  planner 
is  a  major  component  of  WavesWorld.  It  uses  a  skill  network  to  control  the  runtime 
behavior  of  virtual  actors  [40, 43].  A  collection  of  motor  skills  can  be  assembled  into  the 
skill  network', SkillBuilder  is  the  tool  we  use.  for  designing  these  motor  skills.  Once 
completed,  a  motor  skill  is  submitted  to  the  registry/dispatcher  (R/D  in  Figure  2),  which 
will  integrate  the  skill  into  a  pre-existing  skill  net,  or  it  will  create  a  new  skill  net  if  non 
exists.  The  runtime  selection  and  execution  of  motor  acts  depends  on  simulated  sensory 
input  and  the  actor’s  current  behavioral  goals. 

SkillBuilder  was  developed  using  the  virtual  environment  system  3D,  an  interpretive 
toolkit  for  creating  custom  VE  applications  [14].  3D  is  a  testbed  system  designed  to 
support  the  specification  of  objects  and  behaviors  for  virtual  worlds.  It  provides  an 
interpreted  command  language  that  has  many  special  purpose  rendering, 
dynamics,  numerical  math  and  user-interface  functions.  The  3D  command  interpreter  has 
over  700  built-in  and  application-specific  functions,  including  primitives  for 
scene  description  and  rendering,  math,  matrix  and  vector  operations,  general  data 
structure  manipulation,  Denavit-Hartenberg  joint  description,  finite  element  dynamics, 
and  X/Motif  interface  building.  The  3D  front-end  interpreter  uses  the  tool  command 
language  (tel)  from  U.C.  Berkeley  [26].  The  tel  environment  includes  an  interpreted 
command  evaluator  that  allows  rapid  prototyping  of  virtual  worlds  and  VE  interfaces.  All 
the  programming  examples  in  this  paper  are  written  in  tel. 

3.  ARCHITECTURE  OF  SKILLBUILDER 

SkillBuilder  is  part  of  a  project  called  The  Virtual  Sailor,  which  has  as  its  goal  the 
implementation  of  a  VE  that  includes  autonomous  and  interactive  virtual  actors.  The 
attempt  is  to  provide  a  level  of  interaction  with  which  the  end-user  is  familiar  from 
everyday  life,  i.e.  through  language  and  gesture  in  real  time.  To  achieve  task  level 
interaction  with  virtual  actors,  elementary  human  motor  skills  have  to  be  modeled  and 
integrated  into  a  behavior  repertoire  [42,  43]. 

Motor  programs  are  the  source  code  for  each  of  an  actor's  motor  skills.  Finite  state 
machines  have  become  a  common  tool  in  robotics  for  describing  behaviors  [9,  29].  In 
previous  work,  we  have  used  finite  state  machines  to  adaptively  coordinate  and  control 
periodic  behaviors  such  as  walking  [41, 45]. 

SkillBuilder  consists  of  several  components  as  shown  in  Figure  2: 

•  A  skill  template  editor  for  providing  necessary  information  for  the  motor  planner:  a 
set  of  pre-  and  postconditions,  the  effector  system(s)  involved  in  the  skill  (e.g.,  “left  arm” 
or  “right  hand”),  and  a  pointer  to  the  appropriate  motor  program. 

•  A  motor  program  builder  that  generates  motor  programs:  the  source  code  for  an 
actor’s  skill  based  on  finite  state  machines,  i.e.  definite  states  in  if-then-else  clauses. 

•  A  motion  checker  module  that  serves  to  display  and  visualize  the  execution  of 
defined  skills. 

The  main  effort  of  the  work  described  here  is  on  the  motor  program  builder.  We  are 
interested  in  understanding  the  general  structure  and  design  of  motor  programs.  To 
properly  focus  our  research,  we  have  concentrated  on  visually  guided  reaching  and 
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grasping  behaviors.  The  development  environment  3D  provides  all  the  necessary  tools  to 
display  a  virtual  actor  executing  the  defined  skills,  which  is  the  function  of  the  motion 
checker  module. 

In  order  to  maintain  interactive  update  rates,  our  virtual  actors  are  modeled  as 
kinematic  chains  consisting  of  rigid,  polyhedral  links.  It  is  not  our  intent  to  build  the 
most  realistic  looking  virtual  actor,  but  rather  to  study  the  execution  of  skills  and 
to  generalize  the  design  of  motor  programs.  A  rigid  body  model  purely  animated  by 
kinematics  serves  this  purpose  for  the  time  being.  As  fidelity  requirements  increase  for 
specific  applications,  the  kinematic  model  can  later  be  extended  to  simulate  dynamics. 


Figure  2.  Block  Diagram  of  SkillBuilder  components  and  the  Skill  Network 

3.1.  Atomic  skills  and  composite  skills. 

For  a  given  virtual  actor,  we  need  to  know  what  motor  skills  to  implement.  Should 
“signing  a  check”  be  a  primitive  motor  act?  More  likely,  we  would  like  to  decompose 
check-signing  into  component  acts  —  grasping  the  pen,  grasping  and  moving  the  paper, 
moving  the  pen  to  trace  out  one’s  signature,  and  so  on.  Each  of  these  component  acts 
achieves  a  “concrete,  functional  result  or  transformation  describable  as  the  movement  of 
an  object  from  one  place  to  another  or  as  a  change  in  the  state  of  an  object,  (e.g.,  from 
open  to  closed,  on  to  off ... )”  [35]. 

Many  daily  activities  can  be  described  as  composite  actions  consisting  of  some 
number  of  component  acts  that  are  identifiable  as  unitary  operations  on  physical  objects. 
Since  these  components  cannot  be  further  subdivided  at  a  functional  level,  we  will 
consider  these  atomic  skills  to  be  the  building  blocks  for  more  complicated  motor  acts, 
called  composite  skills. 

3.2.  Motor  Program  Design 
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In  order  to  create  a  new  skill  it  is  necessary  to  design  a  finite  state  machine  as  a  model 
for  the  skill.  A  finite  state  machine  can  consist  of  any  number  of  branches  and  feedback 
loops  and  can  be  arbitrarily  complicated.  Oscillators,  servos,  reflexes,  mass/spring 
systems,  collision  detection  and  avoidance  modules,  are  all  components  that  can  be 
incorporated  as  local  motor  programs  into  states  of  a  finite  state  machine,  which  roughly 
serves  to  sequence  the  activity  of  these  modules. 

A  state  consists  of  one  or  more  local  motor  programs,  and  it  is  executed  until  its 
transition  (or  ending)  conditions  are  fulfilled.  Ending  conditions  can  be  geometric 
constraints  (“Do  this  until  the  end  effector  is  closer  to  a  specified  point  than  a  given  goal 
point  precision"),  joint  angle  constraints  (increase/decrease  a  joint  angle  until  it  reaches  a 
specified  target  joint  angle  or  its  limits),  or  even  external  constraints  (“Stop  executing  as 
soon  as  another  motor  program’s  ending  conditions  are  fulfilled”).  Once  the  ending 
conditions  for  a  motor  program  are  satisfied,  its  end  event  will  be  evaluated.  In 
defining  another  motor  program  as  end  event,  several  motor  programs  can  be  chained 
together.  Motor  programs  for  atomic  skills  will  be  classified  as  follows: 

•  Forward  kinematics  skills  are  modeled  by  finite  state  machines  with  usually  more 
than  one  state.  Examples  of  implemented  forward  kinematics  skills  are 
put_arm_on_table  and  reach_ceiling. 

•  Inverse  kinematics  skills  usually  have  just  one  state  and  contain  an  inverse 
kinematics  calculation  procedure  to  synchronize  the  angle  step  size  for  all  local  motor 
programs  involved.  Examples  of  implemented  inverse  kinematics  skills  are 
move_arm_to_goal  anddirect_hand. 

Motor  programs  for  composite  skills  are  composed  of  several  atomic  skills  that  will 
execute  in  parallel,  in  sequence,  or  in  any  combination  of  those  two.  This  will  be 
explained  in  more  detail  in  Section  4.8. 


4.  IMPLEMENTATION 

4.1.  The  Denavit-Hartenberg  Notation 

To  support  the  construction  of,  and  interaction  with,  kinematic  chains  as  needed  for 
robotics  applications,  the  Denavit-Hartenberg  joint  notation  [27]  has  been  implemented  in 
3D.  This  feature  has  been  used  in  this  work  to  build  the  structure  of  a  jointed  figure 
by  defining  kinematic  chains  for  each  of  the  extremities. 

The  Denavit-Hartenberg  notation  is  a  systematic  method  of  describing  the  kinematic 
relationship  between  a  pair  of  adjacent  links  with  a  minimum  number  of  parameters.  The 
relative  position  and  orientation  of  the  two  coordinate  frames  attached  to  those  two  links 
can  be  completely  determined  by  the  following  4  parameters: 
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•  an:  the  length  of  the  common  normal 

•  d  n-  the  distance  between  the  origin  of  joint  n  and  the  common  normal  along  zn+l 

•  an‘-  the  twist  angle  between  the  joint  axes  about  xn 

•  Qn  :  the  angle  between  the  xn-i  axis  and  the  common  normal  measured  about  zn-l 

In  a  human  skeleton  all  degrees  of  freedom  can  be  satisfactorily  modeled  by  revolute 

joints.  Changing  the  angle  0n  will  cause  a  link  n  to  turn  about  the  zn-l  axis.  All 
other  dhparameters  will  remain  constant.  The  relationship  between  adjacent  coordinate 

frames  can  be  expressed  in  a  4  x  4  matrix: 

'cos©,.  -  sin  0,.  cos  ct,.  -sin0,sina(. 
sin0,  cos  0,.  cos  a,.  -  cos  0,.  sin  a,. 

A:  = 

'  0  since,.  cos  a,. 

v  0  0  0 

Thus,  the  position  and  rotation  matrix  of  joint  n  can  be  calculated  by  multiplying  the 
position  and  rotation  matrix  of  a  joint  m  (n>m)  with  all  transformation  matrices  from  each 
joint  in  between  m  and  n  to  its  adjacent  joint: 

M>j  =  Mm  •  Am+1  ‘  A/n+2  *  •••  •  An -1  •  A n  (4.2) 

In  3D  every  defined  Denavit-Hartenberg  joint  n  has  a  dhmatrix  assigned  to  it  which  is 
the  matrix  Mn  from  equation  (4.2).  This  way,  3D  internally  takes  care  of  providing  the 
absolute  transformation  matrices  for  all  body  parts,  depending  on  the  value  of  the 
dhparameters. 

Spherical  joints  can  be  defined  by  a  system  of  three  coincident  joints,  where  no  two 
joint  axes  are  parallel.  When  defining  the  kinematic  chains  for  the  extremities,  a  few 
“dummy”  joints  have  to  be  introduced.  This  is  caused  by  the  fact  that  it  is  not 
always  possible  to  rotate  the  subsequent  coordinate  frame  to  line  up  as  desired,  since  the 

twist  angle  a  only  produces  a  rotation  about  xn-  The  “dummy”  joints  will  not  move  but 
only  serve  to  modify  the  coordinate  frame  orientation. 

4.2.  Graphical  User  Interface 

The  user  interface  of  3D  consists  of  four  kinds  of  dialogs: 

•  Keyboard  input 

•  Manipulators  —  independent  local  or  remote  processes,  communicating  with  3D 
via  UNIX™  sockets  (such  as  glovetool  to  control  a  Dataglove) 

•  Mouse  manipulation  of  objects  inside  the  graphics  window 

•  Interactive  X/Motif  Dialog  Boxes 

The  graphical  user  interface  for  SkillBuilder  makes  use  of  several  menus,  including 
the  standard  3d  Menu,  which  has  been  extended  to  include  a  few  more  features,  and  the 
Views  menu  ,  for  controlling  the  synthetic  camera.  The  Command  Loop  menu  has  been 
implemented  as  a  graphical  dialog  for  SkillBuilder.  The  GUI  is  described  in  detail 
elsewhere  [17]. 

4.3.  Local  Motor  Programs 

The  main  focus  of  this  work  was  put  on  skills  that  can  be  performed  with  the  arm, 
hand,  and/or  fingers,  i.e.  reaching  and  grasping.  Based  on  the  structure  of  a  skeleton  arm, 
the  kinematic  chain  for  the  arm  provides  joints  for  the  clavicle  and  the  scapula  (joints  0- 


a,cos0A 

a,.sin0,. 

4 

1  y 


(4.1) 
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5).  Only  two  of  them,  joint  0  and  joint  1,  are  used  to  move  the  shoulder  forward  and 
backward  as  well  as  up  and  down.  There  are  two  dummy  joints  in  the  arm,  i.e.  joint  9  and 
joint  12.  The  remaining  7  joints  control  the  following  joint  motions:  arm  lifting  sideward, 
arm  lifting  for-  and  backward,  arm  turning  (about  the  upper  arm),  elbow  bending,  hand 
turning,  hand  bending,  and  hand  twisting.  Each  of  those  joint  motions  can  be  invoked  by 
a  corresponding  local  motor  program,  e.g.  bend_elbow  by  50  2  5  causes  the  elbow 
to  bend  by  50  degrees  in  25  timesteps. 

For  the  arm,  the  following  local  motor  programs  are  available: 

•  lif t_arm_sidewards  option  angle  timesteps  renopt 

•  move_shoulder  option  angle  timesteps  renopt 

•  lif t/drop_shoulder  option  angle  timesteps  renopt 

•  lif t_arm_sideward  option  angle  timesteps  renopt 

•  if t_arm_forward  option  angle  timesteps  renopt 

•  turn_arm  option  angle  timesteps  renopt 

•  bend_elbow  option  angle  timesteps  renopt 

•  turn_hand  option  angle  timesteps  renopt 

•  bend_hand  option  angle  timesteps  renopt 

•  twist_hand  option  angle  timesteps  renopt 

The  parameters  to  a  local  motor  program  are  option,  which  specifies  whether  the 
appropriate  joint  should  be  moved  “by”  the  specified  angle  or  “to”  the  specified  joint 
position,  the  joint  angle  (be  it  the  absolute  or  relative  one),  the  number  of  timesteps 
in  which  the  motion  should  be  performed,  and  the  renopt  that  can  be  set  to  “ren”  or 
“noren”  depending  on  whether  the  image  shall  be  updated  on  the  screen,  i.e.  rendered, 
with  every  timestep  or  not. 

Bending  and  turning  or  twisting  inwards  are  considered  to  change  a  joint  angle  in 
positive  direction.  A  fully  extended  or  untwisted  limb  corresponds  to  a  neutral  joint  angle 
of  0°. 

Motor  skills  are  described  in  world  space  joint  axis  coordinates,  and  each  must  be 
converted  to  the  appropriate  Denavit-Hartenberg  parameter,  th zdhtheta  value  (also 

referred  to  as  dhangle  0 ,  dhparameter  0 ,  or  simply  0)  consistent  with  the  above 
convention.  Sometimes,  the  direction  of  dhtheta  has  to  be  reversed  to  convert  into  the 

real  joint  angle,  denoted  as  <E>.  Table  1  shows  a  “-1”  in  the  “dir”  column  if  this  is  the  case. 
Also,  there  might  be  an  offset  between  0  and  <I>  (see  column  “off’  in  the  same  table). 
Finally,  the  conversion  from  <F  to  0  can  be  calculated  by 


0  =  <p  *  dir  +  off  (4.3) 

and  is  noted  in  the  column  named  dhangle  0.  Table  1  also  shows  the  joint  limits  for 
the  joint  angles  <I>  and  their  corresponding  limits  in  the  dhparameter  0  according  to  the 

conversion  equation  (4.3).  To  access  a  current  joint  angle  <F,  there  is  a 
procedure  get  j  ointname_angl  e  provided  for  every  joint  that  reads  the  dhtheta  for 
the  joint  in  question  and  converts  it  back  to  the  real  joint  angle  d>. 

Every  local  motor  program  consists  of  three  main  parts  —  the  joint  limit  test,  angle 
conversion,  and  joint  angle  change,  described  as  follows: 

•  Joint  limit  test.  The  procedure 

in_range  limb  joint_nr  option  angle  min_angle  max_angle 
restpos  off  direc  part  action 
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tests  whether  the  desired  motion  exceeds  the  joint’s  limits.  If  it  does,  the  procedure 
returns  an  appropriate  reachable  angle  closest  to  the  one  specified.  Otherwise  it  returns 
the  angle  itself. 

•  Angle  conversion.  Conversion  from  the  desired  joint  angle  <I>  to  the  dhangle  0, 

depending  on  how  the  option  parameter  is  specified  (Eq.  4.3). 

•  Joint  angle  change.  The  change  of  the  corresponding  dhtheta  value  is  invoked  by  the 

procedure 

move  3. _ joint  limb  joint _ nir  option  dhangle  timesteps 

range_angle  renopt 


which  is  the  core  of  every  local  motor  program.  The  option  parameter  can  be  set  to 
”to”  or  “by”  which  provokes  either  a  movement  to  a  specified  absolute  angle  or  by  a 
specified  relative  angle. 


dhangle 

name 

limits 

dir 

off 

dhangle  0 

0  limits 

shoulder_fb 

3.1.  ©4  40 

3.2. 

3.3. 

-0 

3.4.  40 

01 

shoulder_ld 

3.5.  ++  45 

3.6. 

3.7. 

0 

3.8.  45 

©6 

arm_ls 

-180<->180 

3.9. 

3.10. 

0 

3.11.  180 

07 

armjf 

-90+>190 

3.12. 

3.13. 

-0  +  90 

3.14.-100 

08 

arm_tum 

170<-»175 

3.15. 

3.16. 

0 

3.17.  175 

elbow 

mm 

3.19. 

3.20. 

0  +  90 

3.21.240 

011 

hand_turn 

-40<-»190 

3.22. 

3.23. 

0  +  90 

3.24.  280 

013 

hand_bend 

3.26. 

3.27. 

0 

3.28.  80 

014 

mm 

3.30. 

3.31. 

-0 

3.32.  -35 

Table  1.  Transformation  between  dhangle  and  actual  joint  angles  and  limits. 


Local  motor  programs  to  control  the  finger  and  thumb  joints  are: 


spread_f inger  finger  joint  option  angle 

bend_a_finger_ joint  finger  option  angle 


move_thumb_outwards 
turn_thumb 
move_thumb_down 
bend_thumb_  j  oint 


option  angle 
option  angle 
option  angle 
joint  option  angle 


timesteps  renopt 
timesteps  renopt 
timesteps  renopt 
timesteps  renopt 
timesteps  renopt 
timesteps  renopt 


These  local  motor  programs  are  controlled  by  the  same  parameters  as  the  local  motor 
programs  for  the  arm.  The  spread_f  inger  and  bend_a_f  inger_ joint 
procedures  have  an  additional  parameter  to  specify  the  finger  in  question,  including  the 
thumb. 

As  Rijpkema  and  Girard  observe,  it  is  almost  impossible  for  a  human  to  move  the  last 
link  (joint  4)  of  a  finger  without  moving  the  next  to  last  joint  (joint  3)  and  vice- 
versa,  without  forcing  one  of  the  two  to  move  in  some  unnatural  way  [31]. 

Experimental  data  taken  by  Rijpkema  and  Girard  have  led  them  to  the  assumption 
that  the  dependency  of  those  last  two  joint  angles  could  be  reasonably  approximated  by  a 
linear  relationship: 
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04  «  2/3  *  03  (4.4) 

The  bend_a_f  inger_j  oint  procedure  therefore  only  accepts  ”1”  or  “2”  as  input 
for  its  parameter  joint  that  specifies  the  number  of  the  bending  joint  being  moved.  The 

first  two  bending  joints  of  a  finger  correspond  to  the  dhparameter  02  and  ©3.  04,  the 
dhparameter  corresponding  to  the  third  bending  joint,  will  automatically  be  changed 

according  to  Eq.  4.4  with  a  change  in  03.  The  last  two  links  of  the  thumb  are  coupled 
in  the  same  way. 

3.33.  Concurrent  Execution 

Performing  composite  skills  often  means  that  several  atomic  skills  have  to  be 
executed  concurrently  .  Also,  the  actor  might  have  different  tasks  at  the  same  time,  e.g. 
reach  for  a  glass  (which  is  a  composite  skill  itself)  and  move  the  head  and  eyes  towards 
the  glass.  A  command  loop  implements  concurrent  execution  of  several  procedures. 

The  idea  of  the  command  loop  is  to  keep  a  list  —  the  command  list  —  of  all 
commands  that  should  execute  concurrently,  and  to  loop  constantly  through  the  list. 
Every  procedure  that  should  execute  at  a  particular  time  has  an  entry  in  the  command  list 
at  that  time.  Procedures  on  the  command  list  can  control  procedures  for  specific  motor 
programs,  local  motor  programs,  or  procedures  that  provoke  a  change  in  the  environment 
of  the  virtual  actor,  such  as  procedures  to  move  an  object. 

The  procedure  controlling  the  command  loop  is  the  server.  After  executing  each  of 
the  commands  on  the  command  list  once,  the  image  of  the  screen  is  updated  by  calling 
the  rendering  routine.  Processing  several  different  local  motor  programs  by  small  steps 
gives  the  impression  of  concurrent  movement.  The  command  loop  can  be  started  and  run 
uninterruptedly  or  can  be  progressed  step  by  step. 

For  example,  bending  the  elbow  by  50  degrees  in  25  timesteps  is  realized  by  adding 
the  command 

bend_elbow  2  1 

to  the  command  list,  and  removing  it  after  25  steps  through  the  command  loop. 

A  procedure  that  returns  either  “limit”,  “target”,  or  “stop”  removes  itself  from  the 
command  list.  Beyond  that,  motor  programs  that  control  procedures  posses  the  ability  to 
add  or  remove  other  procedures  from  the  command  list.  The  server  takes  into  account 
that  new  procedures  might  have  been  added  to  the  list,  and  that  they  should  be  executed 
before  the  end  of  the  current  step  through  the  loop. 

Access  procedures  that  are  able  to  add,  insert,  or  remove  commands  from  the 
command  list  are  add_c,  insert_c,  and  rem_c. 

For  convenience,  an  X  menu  is  provided,  i.e.  the  Command  Loop  menu,  that  shows 
the  current  command  list  and  has  buttons  to  start,  stop  or  step  through  the  list.  At  the  end 
of  every  step  through  the  list,  all  new  x  events  are  flushed.  This  way,  the  actor  can  react 
to  any  command  typed  in  the  Do  Command  Once  text  field  at  any  time.  Once 
the  command  loop  is  started,  the  actor  can  only  be  controlled  by  using  the  Command 
Loop  menu. 

The  system  has  an  internal  clock  implemented  as  a  global  variable  that  is  increased  by 
one  timestep  every  step  through  the  command  loop. 

4.4.  Inverse  Kinematics  Algorithms 

For  simple  linkages,  it  is  possible  to  derive  closed-form  solutions  for  the  inverse 
kinematics  problem.  While  the  arm  and  hand  of  our  virtual  actor  are  kinematically 
redundant,  certain  parameters,  such  as  the  location  of  the  elbow  (e.g.,  high  or  low)  during 
reaching  operations  can  be  held  constant,  thus  rendering  the  linkage  functionally  non- 
redundant,  and  making  a  closed-form  solution  possible.  We  have  implemented  a  number 
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of  inverse  kinematics  motor  programs  this  way.  The  interested  reader  is  referred  to  [17] 
for  details. 

The  basic  inverse  kinematics  algorithm  for  the  arm  is  used  to  position  either  the  wrist 
or  the  end  effector  of  the  hand  at  a  specific  point  in  space.  The  end  effector  is 
generally  located  in  the  center  of  the  palm.  This  algorithm  will  be  later  extended  in  order 
to  obtain  appropriate  joint  angles  for  all  joints  involved  to  synchronously  reach  a  target 
position  and  orient  the  end  effector. 

If  the  wrist  is  to  be  positioned,  the  hand  angles  (turning,  bending,  and  twisting)  do  not 
play  a  role.  For  the  positioning  of  the  end  effector  of  the  hand,  constant  hand  angles  are 
assumed  for  now. 

The  basic  inverse  kinematics  algorithm  is  implemented  in  a  procedure  called 
goal_arm_angles_abs  that  takes  two  parameters  as  input:  the  goal_point,  a  three 
dimensional  coordinate  point  in  world  space  coordinates,  and  gp_opt,  the  goal 
point  option  that  can  be  either  set  to  “wrist”  or  “ee”  to  specify  whether  the  wrist  or  the 
end  effector  is  to  be  positioned  at  the  given  goal  point.  The  procedure  returns  the  absolute 
angles,  i.e.  the  arm- Is  angle,  the  arm- If  angle,  the  arm- turn  angle,  and  the 
elbow  angle,  necessary  to  reach  the  goal  point.  The  algorithm  is  divided  into  two 
major  parts : 

•  Calculation  of  the  shoulder-elbow-vector  and,  in  case  gp_opt  is  set  to  “wrist”, 
calculation  of  the  elbow-wrist- vector. 

•  Inverse  kinematics  calculation  to  determine  the  joint  angles. 

Adaptation  to  a  moving  body,  shoulder  movements  and/or  changes  in  the  hand  angles 
will  take  place  by  recomputing  the  inverse  kinematics  algorithm  every  timestep. 

Often  it  is  desired  not  only  to  place  the  end  effector  or  wrist  at  a  specific  point  in 
space  but  also  to  reach  a  specific  orientation  of  the  hand.  Imagine  washing  windows.  The 
end  effector  should  follow  a  looplike  trajectory  in  the  plane  of  the  window,  while  the 
hand  plane  should  stay  parallel  to  the  plane  of  the  window  all  the  time.  Another  example 
is  grasping  an  object.  If  the  actor  grasps  a  glass,  it  should  bring  its  end  effector  together 
with  a  specific  point  on  the  glass  and  simultaneously  align  its  thumb  axis  with  the 
longitudinal  axis  of  the  glass.  Again,  the  interested  reader  is  referred  to  [17]  for  details 
of  the  inverse  kinematics  implementation. 

4.5.  Collision  Avoidance 

The  collision  avoidance  feature  we  have  implemented  is  limited  to  the  generation  of  a 
collision-free  path  for  the  end  effector  when  performing  a  reaching  motion  towards  an 
object  on  the  table. 

According  to  the  average  distance,  the  end  effector  is  expected  to  travel  per  timestep 
(dist_per_timestep),  equidistant  goal  points  along  the  path  are  generated.  The  end 
effector  is  supposed  to  move  sequentially  through  all  these  goal  points  in  order  to 
avoid  collision. 

The  procedure  that  calculates  points  on  a  path  around  a  collision  object  (the 
collobj  parameter),  e.g.,  the  table  in  this  example,  is 

f ind_path_pos  object  collobj  showpath. 

The  showpath  parameter  can  be  set  to  “yes,”  which  will  cause  green  spheres  to  be 
displayed  at  the  calculated  way  points  for  visualization  purposes. 


12 


Zeltzer  and  Gaffron 


SkillBuilder 


Figure  4.  Division  of  space  to  find  a  collision  free  path  (vertical  cross-section) 

The  space  around  the  collision  object  viewed  in  the  yz-plane  is  divided  into  several 
areas  according  to  Figure  4.  The  points  P\  to  P5  will  be  calculated  depending  on  the 
position  of  the  collision  object  and  the  object  that  is  to  be  reached: 

Pi  =  (Xpl .  ymax  collobj  +  ydistl »  Zmin  collobj  *  Zdistl  ) 

P2  =  (  xp2  1  ymax  collobj  +  ydistl  >  Zmax  collobj  +  Zdistl  ) 

P3  =  (Xp3  ,  ymax  collobj  +  ydist2  >  Zmax  collobj  +  Zdist2  ) 

P4  =  (Xp4 ,  ymax  collobj  >  Zmax  collobj  +  Zdist2  ) 

P5  =  ( Xp5 ,  y0bj  +  ydist3  .  Zobj  +  Zdist3  ) 

The  following  values  represent  spatial  constants  to  provide  a  safe  margin  about  the 
arm  and  hand: 

ydistl  =  5.2  (accounting  for  the  distance  between  the  end  effector  and  the  tip  of  the 
middle  finger  plus  a  safety  distance  of  2,  measured  in  inches). 
ydist2  =  5 
ydist3  =  1 
Zdistl  —  1 
Zdist2  =  3.5 
Zdist3  =  0.4 
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Depending  on  the  current  position  of  the  end  effector,  the  first  point  for  the  path  will 
be  chosen.  If  the  end  effector  is  located  in  area  3  for  example,  the  first  point  on  the  path 
will  be  Ps  followed  by  P4,  P5,  and  finally  the  object’s  centroid  PQbj,  see  Figure  5. 


Figure  5.  Collision  free  path  starting  from  an  end  effector  position  in  area  3. 

The  x  values  of  the  path  points  depend  on  the  x  value  of  the  current  end  effector 
position  xee ),  and  the  position  of  the  object  on  the  table  x0bj).  If  the  body  is  very  close  to 
the  table,  and  the  end  effector  of  the  left  hand  is  on  the  right  side  of  the  left  shoulder  joint, 
then  the  path  should  lead  the  end  effector  to  move  just  slightly  to  the  left  of  the  shoulder 
joint  at  Pi: 

if  { { $x_ee  <  $x _ sp )  &  ($y_pl  >  -5)  &  ($x_obj  <  $x_ee) }{ 

set  x__pl  [plus  $x_sp  0.1] 

}  else  { 

set  x_pl  $x_eep 

} 

In  order  to  approach  the  object  with  the  left  hand  from  the  left  side,  xp5  will  be  set  to 

xp5  =  xobj  +  3 

and  the  other  x  values 


14 


Zeltzer  and  Gaffron 


SkillBuilder 


X„'  —X.  X.  +X, 

Xp2  =  Xpi,  xp3  =  xpI  +  p-t  Xp4  =  -p‘2  p5 

In  case  yps  is  greater  then  yp4t  P4  will  be  skipped  completely. 

If  the  end  effector  is  located  in  area  5,  a  new  point  Pnew  will  be  inserted  between  the 
end  effector  and  P5  in  case  xee  is  less  than  xcbj,  see  Figure  6.  The  coordinates  of  Pnew  will 
be 


Pnew  —  (xobj  >  y'obj  +  ydistl  >  Zobj  +  Zdist3  ) 


Figure  6.  Insertion  of  Pnew  in  case  the  end  effector  is  located  in  area  5. 

In  order  to  force  the  end  effector  to  move  along  the  collision  free  path  smoothly, 
equidistant  goal  points  have  to  be  generated  on  the  polygon  that  is  defined  by  all  path 
points  included  in  the  path.  Procedure 

make_equidistantpoints  pointlist 
implements  the  following  algorithm: 

Step  1.  Calculate  the  length  l  of  the  polygon  through  a  point  list  (po, ...,  pn). 

Step  2.  Parameterize  the  polygon,  store  the  parameter  f/  at  each  point  of  the  point  list 
C PO •  to,  ... ,  Pn.tn  =  1). 

Step  3.  Find  the  number  of  goal  points  by  dividing  the  polygon  length  by  the  global 
variable  dis t_per_times tep  and  finding  the  closest  integer  nrgp  =  (int 
(//dist_per_timestep)). 

Step  4.  Adapt  the  stepsize  s  accordingly  (j  =  /  /  nrgp ). 

Step  5.  Set  the  first  goal  point  to  be  the  first  point  from  the  point  list  ( gpo  =  po )• 
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Step  6.  Increase  the  parameter  along  the  polygon  by  the  stepsize  ( tgpj  =  tgpj-l  +  5  ) 
and  check  between  which  points  (from  the  point  list)  the  new  goal  point  gpj  is  located 

(ti  <  tgpj  <  ti+j). 

Step  7.  Calculate  the  coordinates  of  the  new  goal  point  from  the  parameter  tgpj  and 
the  linear  equation  formed  by  pi  and  /?/+;. 

Step  8.  Repeat  steps  6  and  7  until  the  end  of  the  point  list. 

Step  9.  Return  a  list  with  all  goal  points  calculated. 

Finally,  the  procedure  equidis  t an t_path_points  combines  both  previously 
explained  procedures  by  providing  the  pointlist  returned  by  f  ind_path_pos  as  input 
for  make_equidistant_points. 

The  implemented  collision  avoidance  algorithm  just  described  is  specific  for  the  left 
arm  and  guarantees  that  the  hand  will  move  from  its  current  position  to  the  object  without 
colliding  with  a  specified  collision  object,  i.e.  the  table.  However,  the  algorithm  does  not 
yet  guarantee  collision  free  moves  of  the  other  arm  links,  i.e.  the  upper  arm  and 
the  forearm.  In  order  to  avoid  collisions  of  these  arm  links  it  is  necessary  to  specify  an 
appropriate  arm_pose_parameter.  Remember,  changing  this  parameter  will 
influence  the  position  of  the  elbow,  but  not  the  end  effector  position. 

4,6.  Motor  Programs  for  Atomic  Forward  Kinematics  Skills 

A  motor  program  for  an  atomic  skill  can  consist  of  several  tel  procedures,  but  has  at 
least  two:  the  main  motor  program  procedure  and  at  least  one  state. 

The  name  of  the  main  motor  program  procedure  is  the  skill’s  name.  Therefore  the 
execution  of  a  skill  is  invoked  by  typing  the  skill’s  name  in  the  Do  Command  Once  text 
field  of  the  Command  Loop  menu  if  the  command  loop  is  already  running.  Otherwise,  the 
skill’s  name  can  be  typed  at  the  3d>  prompt  and  the  command  loop  can  be  started 
thereafter  by  clicking  the  Go  button  in  the  Command  Loop  menu. 

The  task  of  the  main  motor  program  procedure  is  to  check  whether  the  initial 
conditions  for  a  skill  are  fulfilled  and  to  add  the  procedure  for  the  first  state  to  the 
command  list.  In  case  the  initial  conditions  are  not  fulfilled,  the  procedure  will  quit  and 
print  out  a  warning  message.  The  general  structure  of  such  a  main  motor 
program  procedure  looks  like  the  following: 

proc  skill_name  (args  {end_event  {}}  }  { 

# 

#  Initial  condition 

# 

set  conditionO  [ condi tion_procedure  args_C0 ] 
if  $conditionO  then  { 

global  skill_name_e nd_event 

set  skill_name_end._event  $end_event 

add_c  [list  skill_name_statel  args_l] 

}  else  { 

echo  Sorry,  the  initial  conditions  for  executing  this  skill 
are  not  given! 

} 

return 

} 
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“args”  stands  as  a  place  holder  for  arbitrarily  many  arguments  that  the  procedure  might 
need,  for  instance  as  input  to  the  condition  procedures,  or  for  one  of  the  following  states. 
The  parameter  end_event  is  contained  in  any  atomic  skill. 

For  every  state  in  a  finite  state  machine  (refer  to  Figure  3),  there  is  a  separate 
procedure  named  skill_name_stateM,  where  M  is  the  number  of  the  respective  state.  The 
following  shows  the  general  structure  of  such  a  procedure  for  an  intermediate  state: 

proc  skill_name_stateM  {args_M}  { 

# 

#  State  ending  condition 

# 

set  conditionMa  [  condition_procedure_Ma  args_CMa] 
set  conditionMb  (  condition _procedure_Mb  args_CMb ] 
if  $ conditionMa  then  { 

add_c  [list  skill_name_ stateN  args_N] 
return  stop 

} 

if  $conditionMb  then  { 

add_c  [list  skill_name_ stateP  args_P ] 
return  stop 

} 

local_motor_programMl  by  angle 
local_motor _programM2  by  angle 

local_motor_programMn  by  angle 
return 

Every  state  is  determined  by  several  state  transition  conditions,  and  a  number  of  local 
motor  program  procedures  that  cause  different  limbs  to  move  concurrently  while  a  state  is 
being  executed.  As  long  as  none  of  the  transition  conditions  of  a  state  is  true,  the  state 
remains  on  the  command  list  and  all  local  motor  program  procedures  belonging  to  that 
state  are  executed  once  every  step  through  the  command  loop. 

In  the  example  procedure,  the  state  M  branched  out  to  either  state  N  or  state  P, 
depending  on  whether  condition  Ma  or  condition  Mb  becomes  true.  Accordingly,  the 
state  N  or  state  P  procedure  will  be  added  to  the  command  list.  In  general,  a  state  could 
have  any  number  of  transition  conditions  which,  by  themselves,  could  consist  of  a 
compound  of  different  subconditions).  Any  network  of  “and”  and  “or”  connections 
between  the  subconditions  can  make  up  a  state’s  transition  condition. 

If  a  state  procedure  returns  “stop”,  the  state  is  automatically  removed  from  the 
command  list. 

The  end_event  parameter  is  passed  to  the  main  motor  program  procedure.  This 
parameter  is  made  accessible  to  the  last  state  X  by  being  defined  as  a  global  variable  in 
the  procedure  skill_name.  As  a  default,  the  end_event  is  set  to  “{ }”  so  that  the 
evaluation  of  a  skill’s  end  event  will  have  no  effect.  The  procedure  then  goes  on  to  the 
command  return  stop  which  forces  the  skill  to  end,  and  causes  the  last  state  to  be 
removed  from  the  command  list. 

There  are  cases  where  a  changing  environment  makes  it  necessary  to  perform  the 
same  skill  over  and  over  again,  because  once  the  skill  is  fulfilled,  the  initial  conditions 
might  come  true  again.  By  defining  the  sk.iil_na.me  itself  as  end_event,  the  skill’s  first 
state  will  replace  the  skill’s  last  state. 

Using  these  techniques,  we  have  implemented  an  adaptive  reaching  algorithm  for  a 
moving  target  object.  Once  the  object  is  reached,  the  ending  condition  will  come  true,  but 
the  reaching  state  would  not  be  removed  from  the  command  list.  Any  time  the  object 


1  noren 
1  noren 

1  noren 
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moves  farther  away  in  one  timestep  than  a  given  goal-point-precision,  the  reaching 
algorithm  would  be  activated  again. 

4.7.  Motor  Programs  for  Atomic  Inverse  Kinematics  Skills 

Inverse  kinematics  skills  consist  of  only  one  state  containing  all  local  motor  programs 
involved  in  a  specific  movement.  The  inverse  kinematics  calculation  routine  synchronizes 
the  angle  steps  of  all  these  local  motor  programs,  depending  on  how  many  timesteps 
the  skill  should  take  until  it  terminates. 

Besides  the  parameter  end_event  which  is  contained  in  any  atomic  skill,  the 
inverse  kinematics  skills  will  all  contain  the  parameters  time_opt  and  timesteps.  If 
the  parameter  time_opt  is  set  to  “1”,  the  time  when  the  execution  of  the  skill  has  to  be 
finished,  end_time,  has  to  be  calculated  depending  on  the  state  of  the  internal  clock. 
During  the  execution  of  the  IK_skill_name_stetz\,  the  end_time  will  be  compared 
with  the  internal  clock  ic,  and  the  number  of  the  remaining  timesteps  will  be  set 
accordingly.  It  will  be  guaranteed  that  the  number  of  timesteps  never  drops  below  one, 
even  though  the  end_time  for  the  skill  might  be  exceeded. 

Usually,  and  as  a  default,  the  time_opt  parameter  will  be  set  to  “0”,  which  means 
that  the  remaining  number  of  timesteps  will  be  calculated  again  every  step  by  a 
times teps_calculation_procedure  to  control  the  end  effector  or  joint 
velocities.  For  example,  the  procedure  calc_dis t_times teps  calculates  the 
timesteps  necessary  to  overcome  a  certain  distance  depending  on  the  value  of  the  global 
variable  dist_per_timestep  that  specifies  the  average  distance  an  end  effector 
should  travel  per  timestep.  This  global  variable  can  be  changed  any  time,  even  during  the 
execution  of  the  skill,  using  the  procedure  dps. 

The  inverse_kinematics_calculation_procedure  first  calculates  the 
final  absolute  joint  angles  for  the  involved  limbs,  then  converts  the  absolute  angles  into 
relative  joint  angles  necessary  to  reach  the  final  goal  depending  on  the  current  state  of  the 
joints),  and  finally  returns  the  relative  angle  steps  every  joint  has  to  move  during  the  next 
step.  The  local  motor  programs  will  only  be  activated  if  the  corresponding  angle  step  size 
is  greater  than  0.001. 

4.8.  Motor  Programs  for  Composite  Skills 

Atomic  skills  can  be  combined  into  composite  skills  in  three  different  ways:  parallel 
independent,  parallel  dependent,  and  sequential,  as  follows: 

•  Parallel,  independent: 

Two  or  more  atomic  skills  are  added  to  the  command  list  at  the  same  time  so  that  they 
start  executing  synchronously.  Every  atomic  skill  executes  independently  from  the  others. 
That  means  that  it  will  be  removed  from  the  command  list  the  moment  it  satisfies 
its  ending  conditions,  independent  of  the  duration  of  the  other  atomic  skills  that  might  be 
different.  See  Figure  7  for  the  finite  state  machine  model.  The  implementation  is  fairly 
easy: 

proc  composite_skill_name  {args}  { 
skill_namel  args_Sl 
skill_name2  args_S2 
return 

} 
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Figure  7.  Model  of  a  composite  skill  composed  of  parallel, 
independent  atomic  skills. 


•  Parallel,  dependent: 

As  in  the  former  case,  all  atomic  skills  will  start  executing  synchronously,  but  in  this 
case,  their  ending  conditions  are  coupled.  That  is  to  say,  all  atomic  skills  corresponding  to 
the  same  composite  skill  will  be  removed  from  the  command  list  only  when  they  all 
satisfy  their  corresponding  ending  conditions  at  the  same  time  (  see  Figure  8).  To  couple 
the  ending  conditions,  all  atomic  skills  but  one  will  remain  on  the  command  list,  after 
satisfying  their  individual  ending  conditions.  One  of  the  atomic  skills  has  to  evaluate  the 
combination  of  all  individual  ending  conditions  and,  in  case  they  are  all  satisfied,  remove 
the  atomic  skills  altogether  from  the  command  list. 


Figure  8.  Model  of  a  composite  skill  composed  of  parallel, 
dependent  atomic  skills. 

Keeping  an  atomic  skill  on  the  command  list  can  be  realized  by  defining  the  name  of 
that  particular  atomic  skill  as  its  own  end_event  as  explained  earlier.  This  way,  an 
atomic  skill  will  add  its  first  state  again  to  the  command  list  upon  completion. 

The  evaluation  of  all  ending  conditions  can  be  done  in  an  extra  procedure  defined  for 
that  particular  purpose.  The  skill  that  has  the 
ending_condition_evaluation_procedure  as  one  of  the  commands  of  its 
end_event  removes  the  other  skills  directly  from  the  command  list  by  using  the 
r  em_c  procedure,  and  will  be  removed  itself  by  returning  “stop”. 

proc  compos it e_skill_name  { args }  { 
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skill_namel  args_Sl  { skill_namel  args_Sl } 
skill_name2  args_S2  { skill_name2  args_S2 } 
skill_name3  args_S3  { 

ending_condition_evaluation_procedure 
rem_c  { skill_namel *} 
rem_c  { skill_name2 *} 

} 

return 

} 


•  Sequentially: 

Feeding  another  skill_name2  as  end_event  to  a  skill_namel  will  link  those  two 
skills  to  execute  sequentially. 

proc  compos it e_skill_name  { args }  { 
skill_namel  args_Sl  { 
skill_name2  args_S2 

} 

return 

} 


A  more  complex  network  of  atomic  skills  making  up  a  composite  skill  is 
shown  in  Figure  9.  The  implementation  follows: 

proc  compos it e_skill_name  {  args  }  { 
skill_namel  args_Sl  { 
skill_name2  args_S2  {} 

IK_skill_name3  args_S3  time_opt  timesteps  { 
skill_name4  args_S4  { 
skill_name5  args_S5  {} 
skill_name6  args_S6  {} 

IK_skill_name7  args_S7  time_opt  timesteps  {} 


} 


} 


return 
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Note  that  the  end  event  can  consist  of  a  list  of  commands.  In  this  way  it  is  possible  to 
start  several  atomic  skills  in  parallel,  upon  completion  of  a  previous  skill.  The  skills  that 
should  execute  sequentially  are  nested  as  each  other’s  end  events. 

In  this  section  we  have  shown  how  to  generalize  the  generation  of  motor  programs 
using  finite  state  machines.  Every  motor  program  consists  of  a  compound  of  tel 
procedures,  i.e.  one  for  each  state  and  a  main  motor  program  procedure  that  checks  the 
initial  conditions.  The  definition  of  each  state  consists  mainly  of  the  identification  of 
local  motor  programs  and  a  definition  of  the  ending  conditions  for  that  particular  state. 
Local  motor  programs  define  the  action  of  a  state  and  the  ending  conditions  define  what 
conditions  have  to  be  satisfied  in  order  to  proceed  to  the  next  state. 

A  distinction  has  been  made  between  forward  and  inverse  kinematics  skills.  It  has 
been  demonstrated  how  to  compose  more  complex  skills  from  atomic  skills  in  a  parallel 
or  sequential  manner,  or  a  combination  of  both. 

During  the  development  of  the  general  structure  of  motor  programs  it  became  clear 
that  there  is  a  variety  of  arguments  that  might  have  to  be  passed  to  specific  skills  and  a 
few  arguments  that  are  always  the  same,  e.g.  the  end_event  has  to  be  specified  for 
every  skill.  Other  arguments  might  be  the  goal_point,  i.e.  the  final  position  the  end 
effector  should  reach  in  a  skill  move_arm_to_goal. 

The  next  section  will  explain  more  about  condition  procedures  that  can  check  initial, 
as  well  as  ending,  conditions  and  introduce  some  of  them. 

4.9.  Condition  Procedures 

Condition  procedures  are  Boolean  procedures  that  check  whether  certain  conditions 
are  fulfilled  or  not,  and  return  either  “1”  or  “0”  accordingly. 

Those  procedures  can  be  called  to  either  check  the  initial  conditions  for  a  skill  or  to 
check  the  transition  (or  ending)  conditions  of  a  state  in  order  to  decide  when  to  go  on  to 
the  next  state  and  which  state  should  be  executed  next.  In  general,  the  same  procedures 
that  function  as  ending  conditions  can  also  function  as  initial  conditions.  A  skill  will  then 
only  be  invoked  by  the  skill  network  if  the  initial  conditions  specified  in  the  skill  template 
are  satisfied.  For  now  it  will  be  taken  for  granted  that  the  initial  conditions  for  a  skill  are 
satisfied. 

There  is  a  great  variety  of  ending  conditions.  Most  of  them  can  be  classified  in  the 
following  way,  where  conditions  1-8  represent  geometric  conditions,  and  9  and  10  do 
not  imply  a  geometric  calculation.  In  this  discussion,  the  following  notation  will  be  used: 
bounding  box  of  an  object  = 

( xmin_obj>  ymin_obj>  Zmin_obj>  xmax_obj  ymax_obj>  Zniax_obj)> 
bounding  box  of  a  body  part  = 

( xmin_bp>  ymin_bp>  Zmin_bp>  xmax_bp>  ymax_bp>  Zmaxjbp )> 
joint  position  =  ( xjoint >  yjoint>  zjoint)  >  and 
target  position  =  (xtarget,  ytarget.  ztarget)- 

Condition  1:  Geometric  relationships  between  a  joint  and  an  object 

Comparing  the  location  of  a  joint  with  the  bounding  box  of  an  object. 

* x joint  >  xmax_obj,  y joint  >ymax_obj >  or  zjoint  >  Zmax_obj- 

*  x joint  <  xmin_obU  yjoint  <  ymin_obj>  or  zjoint  <  Zmin_obj- 

•  Joint  within  specified  distance  from  object. 

9.1.,  “Is  the  wrist  under  the  table,  above  the  table  or  in  front  of  it?” 

Condition  2:  Geometric  relationships  between  a  body  part  and  an  object 

Comparing  the  bounding  boxes  of  a  body  part  and  an  object. 

•  xmin_bp  >  xmax_obj>  yminjbp  >  ymax_obj>  or  Zminjbp  >  Zmax_obj- 

*  xmax_bp  <xmin_obji  ymaxjbp  <  ymin_obj>  or  Zmaxjbp  <Zmin_obj- 
“Is  the  glass  located  to  the  right  or  to  the  left  of  the  forearm?*’ 
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Condition  3:  Geometric  relationship  between  joints 
Comparing  the  location  of  two  joints. 

• xjointl  =xjoint2 »  y joint  1  =yjoint2  or  zjointl  =  Zjoint2- 

•  x joint  1  <xjoint2>  yjointl  <>joint2.  or  Zjointl  <zjoint2- 

•  #  xjointl  >  xjoint2,  yjointl  >  yjoint2 .  or  zjointl  >  zjoint2- 
“Is  the  wrist  located  below  or  above  the  elbow?”  or  ”Is  the  elbow  positioned  to  the 
right  or  to  the  left  of  the  shoulder  joint?” 

Condition  4:  Location  of  a  joint 

Comparing  the  location  of  a  joint  with  a  target  position. 

•  x joint  -  xtargeU  y joint  -  ytargeU  or  zjoint  =  Ztarget- 

•  x joint  <  xtargeU  yjoint  <  ytargeU  or  zjoint  <  Ztarget- 
•xjoint  >  ^target,  yjoint  >  ytarget.  or  zjoint  >  ztarget- 

•  Joint  position  ( xjoint .  yjoint  zjoint)  =  target  ( xtarget  ytarget.  ztarget)- 

•  Joint  position  within  specified  distance  of  object. 

“Is  the  wrist  positioned  at  a  height  of  z=75  ?”  or  “Is  the  end  effector  positioned  at  a 
specified  goal  point?” 

Condition  5:  Orientation  of  a  body  part 

Measuring  the  angle  a  body  part  axis  makes  with  a  target  axis. 

•  Body  part  orientation  =  target  orientation 

•  Body  part  orientation  aligned  with  a  specified  axis  of  an  object 

“Does  the  forearm  point  in  the  target  direction?”  or  “Is  the  thumb  vector  aligned  with 
the  longitudinal  axis  of  the  glass?” 

Condition  6:  Angular  constraints 

Comparing  a  joint  angle  with  a  target  angle. 

•  Joint  angle  =  target  angle 

•  Joint  angle  >  target  angle 

•  Joint  angle  <  target  angle 

•  Configuration  of  joint  angles  of  a  limb  =  target  configuration 

“Is  the  elbow  angle  <  130  degrees?”  or  ”Is  the  arm  positioned  in  the  target 
configuration?” 

Condition  7:  Contact  between  a  body  part  and  an  object 

Checking  for  any  collision  between  a  body  part  and  an  object. 

“Do  the  fingers  touch  the  table?”  or  ’’Does  the  left  toe  touch  the  ground?” 

Condition  8:  Contact  between  body  parts 

“Does  the  palm  touch  the  lower  body?”  or  ”Do  the  upper  right  and  the  upper  left  leg 
touch  each  other?” 

Condition  9:  Change  in  one  of  the  global  variables  of  SkillBuilder 
Checking,  for  example,  whether  the  arm__pose_parameter  changed  since  the  last 
timestep. 

Condition  10:  Ending  condition  of  another  skill  currently  being  executed  is 
satisfied 

The  condition  mentioned  last  can  be  used  for  composite  skills  composed  of  parallel, 
dependent  atomic  skills. 
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An  ending  condition  can  be  composed  of  any  number  of  condition  procedures  by 
connecting  them  logically. 

In  many  cases,  a  condition  procedure  is  very  general  and  is  made  suitable  for  a 
specific  skill  state  by  passing  the  appropriate  arguments  to  the  procedure.  However, 
sometimes  condition  procedures  have  to  be  generated  that  are  very  specific  for  one  skill. 
Those  procedures  can  be  of  any  of  the  above  mentioned  kinds  and  will  have  to  be 
developed  on  an  ad  hoc  basis  when  defining  new  skill  states. 

For  example,  if  a  state  should  only  branch  to  state  X,  if  (condition  1  and  condition  2 
and  (condition  3  or  condition  4))  are  true,  its  ending  condition  might  look  as  follows: 

5.  VISUALLY  GUIDED  MOTION 

In  this  work,  we  do  not  simulate  visual  perception.  Rather,  we  simulate  visually 
guided  motion  by  providing  object  names,  positions  and  orientations  to  motor  skills. 

5.1.  Orienting  a  Body  Part 

All  motor  programs  for  skills  to  orient  a  body  part,  e.g.  pointing  with  the  forearm  in  a 
specified  direction,  or  aligning  the  thumb  axis  with  an  object  axis,  are  based  on  one  of 
the  inverse-kinematics-calculation-procedures.  They  belong  to  the  category  of  motor 
programs  for  atomic  inverse  kinematics  skills.  The  number  of  timesteps  taken  by  these 
skills  can  be  set  to  a  constant,  or  it  can  be  recalculated  for  each  timestep  based  on  the 
angular  and  linear  distances  remaining  to  be  covered.  In  future  we  will  fill  in  these 
constants  based  on  biomechanical  data  consistent  with  observed  human  behaviors.  In 
addition,  Lee  et  al,  have  shown  how  to  implement  physically  based  motor  skills  based  on 
human  motor  performance  models  [21]. 

More  complicated  composite  skills  will  often  have  one  or  more  of  the  atomic  orient- 
body-part  skills  as  their  elements.  The  name  of  orient-body-part  skills  often  starts  with 
direct_*  *  *.  An  example  of  how  to  incorporate  the  direct_upper_arm, 
direct_f  orearm  and  direct_hand  skills  follows  in  a  skill  called  wave_hand: 

wave_hand  end_event. 

This  skill  is  composed  mainly  of  orient-body-part  skills  (see  Figure  10).  The  arm  first 
reaches  specific  orientations  for  the  upper  arm  and  the  forearm.  Once  the 
target  orientation  for  the  upper  arm  is  reached,  the  hand  is  oriented  so  that  the  palm  axis 
points  forwards  (as  viewed  from  the  body).  The  target  for  the  forearm  orientation  alters 
between  two  orientations  while  the  hand  angles  are  constantly  adjusted  so  as  to  keep  the 
specified  palm  orientation. 

Hand  waving  is  an  oscillating  skill.  All  oscillating  skills  need  to  add  a  control 
procedure  to  the  command  list  (e.g.  wave_hand_control  n  this  case)  that  constantly 
checks  whether  the  skill  should  keep  executing.  The  hand  waving  skill  can  be  stopped 
by  typing  stop_waving  (in  the  Do  Command  once  text  field).  This  will  set  the  global 
variable  stop_waving  to  “1”  so  that  the  wave_hand_control  procedure  will 
remove  all  atomic  skills  invoked  by  wave_hand  from  the  command  list,  including 
itself.  Furthermore,  it  will  evaluate  the  end  event  of  wave_hand. 
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Figure  10.  Composite  skill  wave_hand. 

This  way  it  is  possible,  to  bring  the  arm  back  in  its  original  position  by  defining  the 
skill 

move_arm_to_config  config  time_opt  times teps  end_event 

as  end  event  of  wavejhand.  The  parameter  config  could  be  set  to  “$org”  which  is  the 
global  variable  in  which  the  original  arm  configuration  is  stored.  The  call  would  look  as 
follows: 

wave_hand  { 
global  org 

move_arm_to_config  $org  0  0  { } 

} 

5.3.  Integrated  Reaching  and  Grasping 

We  explicitly  combine  reaching  and  grasping  since  experiments  with  human  subjects 
have  demonstrated  that  while  reaching  for  objects,  humans  begin  shaping  the  hand  into 

grasping  configurations  that  roughly  match  the  shape  of  the  target  object  [20,  28]. 
Therefore,  reaching  and  grasping  together  form  an  atomic  skill.  Grasping  can  be  easily 
integrated  into  the  composite  skill  reach_obj  ect_along_path  by  adding  the 
atomic  skill  bring_all_f  ingers_in_open_grasp_conf  ig.  The  skill  to  open  the 
fingers  should  start  executing  in  parallel  with  one  of  the  move_arm_to_goal 
skills  depending  on  how  many  timesteps  the  finger  opening  should  take.  Defining  the 
atomic  skill  grasp_obj  ec  t  as  end  event  will  make  the  fingers  grasp  the  object  after  it 
has  been  approached.  See  Figure  11. 

5.4.  Head  and  Eye  Controller 

A  controller  for  the  head  and  eye  movements  has  been  developed  by  Prof.  Sunil 
Singh  and  his  student  Dan  Popa  at  Dartmouth  College,  and  has  been  integrated  into 
SkillBuilder  [13]. 

It  is  the  task  of  the  head-eye  controller  to  track  a  target  object  moving  in  a  3- 
dimensional  space.  The  controller  has  been  implemented  as  a  manipulator  —  i.e.,  an 
independent  local  or  remote  process,  communicating  via  UNIX™  sockets)  written  in  C, 
communicating  with  the  tel  procedures  of  the  SkillBuilder . 

The  head,  represented  by  a  model  of  a  skull,  is  placed  on  the  neck  at  joint  9  of  the 
dhchain  of  the  spine  and  is  capable  of  limited  motion  about  three  intersecting  coordinate 
axes,  like  a  spherical  wrist  in  robot  manipulators.  The  eyes  are  represented  by  two  white 
balls.  Two  smaller  blue  balls  inside  the  white  balls  represent  the  pupils. 
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Singh  and  Popa  make  use  of  the  fact  that  a  target  can  be  characterized  by  its 

coordinates  with  respect  to  a  fixed  frame  and  by  an  angle  a,  which  represents  the  angular 
orientation  of  the  object  with  respect  to  the  rotated  axis  normal  to  the  skull.  They 
determine  the  rotation  matrix  of  the  skull  relative  to  the  fixed  frame  by  using  the  Euler 
formalism,  and  solve  the  formulated  inverse  kinematics  equations  for  the  rotation  angles 
around  the  fixed  frame. 

Since  it  is  not  intended  to  move  the  head  towards  a  target  at  all  times,  but  rather  move 
only  the  eyes  or  both,  they  make  the  following  considerations: 

•  The  fovea  of  both  eyes  can  be  independently  directed  to  the  object  of  interest 
(vergence  eye  movements). 

•  If  the  object  is  “almost”  in  front  of  the  eyes,  the  eyes  move  only  in  a  quick,  gaze- 
shifting  response,  called  a  saccade. 

•  Both  the  skull  and  the  eyes  have  a  relative  rotation  range  which  cannot  be 
exceeded. 

•  If  the  object  to  be  tracked  is  moving  outside  of  the  range  for  “comfortable”  eye 
movements,  the  head-eye  motions  must  be  carefully  distributed  between  the  head  and  the 
eyes. 

While  the  head  control  unit  uses  purely  inverse  kinematics,  the  eye  control  unit 
explicitly  accounts  for  the  motion  dynamics  of  the  object  and  the  human  occulomotor 
system. 

SkillBuilder  provides  the  following  atomic  skills  to  invoke  head/eye  motions: 

head_track  object  end_event 

head_straight  time_opt  timesteps  end_event 

head_move  object  time_opt  timesteps  end_event 

Each  of  those  skills  will  put  the  appropriate  procedures  on  the  command  list  and  will 
be  executed  only  if  the  command  loop  is  running.  The  head_move  skill  will  cause  the 
head  and  eyes  to  move  the  gaze  point  towards  the  specified  obj  ect  in  the  given  number 
of  timesteps  and  evaluate  its  end_event  upon  completion  of  the  skill. 

Keeping  the  focus  of  the  head  and  eyes  on  a  moving  object  can  be  done  by  invoking 
the  head_t rack  skill.  It  is  useful  to  define  head_track  as  end  event  of 
head_move,  in  order  to  slowly  move  the  focus  towards  an  object  before  tracking  it. 
head_track  is  a  continuous  skill,  which  means  it  will  only  stop  executing  when  it 
is  explicitly  stopped  by  calling  the  procedure  s  top_head_track. 

The  skill  head_straight  is  nothing  else  but  a  call  to  the  head_move  skill  with  a 
dummy  object  placed  at  the  point  the  head  should  look  at. 

Figure  12  shows  the  virtual  actor  with  the  skull  looking  at  a  ball  in  its  hand. 

A  number  of  motor  programs  for  simulating  visually  guided  behavior  are  described  in 
detail  in  [17]. 

6.  FUTURE  WORK 

Local  motor  programs  exist  only  for  the  left  arm  and  all  fingers  and  the  thumb  of  the 
left  hand.  However,  creating  local  motor  programs  for  all  other  joints  will  be 
straightforward,  given  the  templates  from  the  implemented  ones. 

The  classification  of  condition  procedures  given  Section  4.9  suggests  several  general 
conditions  that  have  not  yet  been  implemented.  As  the  complexity  and  variety  of  skills 
increases  more  condition  procedures  appropriate  for  those  skills  will  have  to  be 
generated. 

There  is  much  current  research  in  finding  the  best  collision  free  path  through  a 
cluttered  environment.  Especially  in  robotics,  many  algorithms  for  a  manipulator  with 
several  links  have  been  suggested.  As  the  virtual  environment  for  the  actor  gets  more 
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and  more  complex,  it  will  be  important  to  integrate  a  more  general  path  finding  algorithm 
and  possibly  extend  it  with  a  collision  detection  feature.  It  will  be  necessary  not  only  to 
guarantee  a  collision  free  path  for  the  end  effector,  i.e.  the  palm,  but  also  all  links  of  a 
moving  limb. 

As  the  fidelity  requirements  of  the  applications  increase  it  will  be  necessary  to  include 
dynamics  in  the  simulation  to  make  it  look  more  realistic.  This  would  require  modifying 
certain  local  motor  programs  for  dynamic  simulation,  and  updating  corresponding  motor 
programs  and  finite  state  machines  to  accommodate  physically  based  initial  and  transition 
conditions. 

The  movements  of  the  virtual  actor  performing  a  skill  could  be  compared 
with  experimental  data  from  human  subjects  performing  the  same  tasks,  in  order  to 
validate  the  motor  programs. 

By  distributing  computational  processes  over  a  network  of  machines  the  performance 
could  be  made  faster  and  made  look  more  realistic. 

It  would  be  especially  nice  if  a  motor  skill  designer  would  not  have  to  write  tel 
source  code.  Because  the  general  structure  of  motor  programs  has  been  defined  in  this 
research,  it  will  easily  be  possible  to  generate  a  motor  program  builder  for  atomic  as  well 
as  composite  skills.  A  motor  program  builder  would  prompt  the  user  for  all  necessary 
information  to  create  the  source  code,  i.e.  the  motor  programs,  automatically. 

One  of  the  most  important  remaining  issues  is  to  make  the  SkillBuilder  easier  to 
handle.  Once  the  generation  of  motor  programs  has  been  automated,  a  visual 
programming  language  for  defining  motor  skills  should  be  implemented.  Furthermore,  all 
available  skills  should  be  able  to  be  invoked  by  choosing  an  item  from  a  graphical  list, 
taking  its  arguments  from  another  graphical  menu.  The  values  of  all  global  parameters 
should  be  displayed  and  able  to  be  changed  by  graphical  means. 

7.  CONCLUSION 

We  have  described  the  development  of  SkillBuilder,  a  software  system  that  can 
function  as  a  design  tool  for  motor  programs  —  the  source  code  of  skills  a  virtual  actor 
can  perform. 

A  human  figure  model  was  generated  consisting  of  rigid  body  parts  that  are  linked 
together  as  several  kinematic  chains  using  the  Denavit-Hartenberg  notation.  Every  joint 
of  the  figure  can  be  controlled  by  a  corresponding  local  motor  program. 

Motor  skills  are  classified  as  atomic  and  composite  skills;  atomic  skills  are  further 
classified  into  forward  and  inverse  kinematics  skills.  Atomic  skills  are  modeled  by  finite 
state  machines  that  control  the  execution  of  a  skill.  The  composition  of  more  complex 
composite  skills  has  been  demonstrated  by  linking  atomic  skills  in  parallel  or  in 
sequence,  or  in  a  combination  of  both. 

It  has  been  shown  how  to  generalize  the  generation  of  motor  programs.  Every  motor 
program  consists  of  a  set  of  procedures,  each  defining  one  state  of  a  finite  state  machine. 
A  state  consists  of  one  or  more  local  motor  programs,  which  specify  and  control  the 
action  to  be  taken  during  the  execution  of  that  state,  and  a  definition  of  the  transition 
conditions  that  must  be  satisfied  in  order  to  proceed  to  the  next  state. 

Transition  conditions  have  been  classified,  and  a  selection  of  condition  procedures 
has  been  implemented.  In  many  cases,  a  condition  procedure  is  very  general  and  is  made 
suitable  for  a  specific  skill  state  by  passing  the  appropriate  arguments  to  the  procedure, 
for  one  skill. 

Parallel  execution  of  several  motor  programs  has  been  implemented  by  the  command 
loop,  which  continually  evaluates  all  procedures  currently  on  the  command  list.  Thus, 
every  motor  program  currently  executing  has  an  entry  of  one  of  its  states  in  the 

command  list. 

The  focus  of  motor  skill  development  was  on  simulated  visually  guided  reaching 
and  grasping  skills  using  the  left  arm.  Reaching  for  an  object  involves  inverse  kinematics 
calculations.  A  cylindrical  grip  has  been  implemented  for  which  the  thumb  axis  (i.e.  an 
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axis  in  the  palm  plane  pointing  towards  the  thumb)  needs  to  be  aligned  with  the 
longitudinal  axis  of  the  object  to  be  grasped. 

A  collision  avoidance  algorithm  has  been  implemented  that  guarantees  that  the  hand 
will  move  from  its  current  position  to  the  object  without  colliding  with  a  specified 
collision  object,  i.e.  the  table.  However,  the  algorithm  does  not  yet  guarantee  collision 
free  moves  of  the  other  arm  links,  i.e.  the  upper  arm  and  the  forearm.  In  order  to  avoid 
collisions  of  these  arm  links  the  relative  elbow  position  can  be  influenced  appropriately 
by  changing  the  arm_pose_parameter. 

The  reaching  and  grasping  skills  have  been  successfully  demonstrated  as  realtime 
response  to  a  direct  interaction  between  the  human  VE  participant  and  the  virtual  actor.  A 
glass  positioned  on  a  table  located  in  front  of  the  virtual  actor  can  be  ’’picked  up”  and 
positioned  anywhere  else  using  a  Dataglove.  When  “asking”  the  actor  to  grasp  the  glass, 
an  appropriate  collision-free  path  is  generated  and,  if  desired,  shown  on  the  screen.  The 
actor  starts  forming  an  open  grasp  position  with  the  fingers  while  reaching  for  the  glass 
along  the  collision  free  path.  Once  the  glass  has  been  reached  the  actor  wraps  the  fingers 
around  the  glass  until  a  contact  has  been  detected  between  each  of  the  fingers  and  the 
glass.  When  “picking  up”  the  glass  again,  the  actor  may  be  “asked”  to  hang  on  to  it.  The 
actor  always  adjusts  the  angles  of  the  hand  so  that  it  keeps  the  same  orientation  with 
respect  to  the  glass.  This  way  it  is  also  possible  to  demonstrate  a  drinking  actor.  Please 
see  Figure  13,  and  the  accompanying  video  tape. 

The  focus  was  deliberately  put  on  visually  guided  reaching  and  grasping  skills,  rather 
than  on  the  attempt  to  implement  a  range  of  behaviors.  The  exercise  has  led  to  a  better 
understanding  of  the  issues  involved  in  designing  the  needed  software  tools  that  will 
serve  to  extend  SkillBuilder  with  a  range  of  representative  motor  skills. 
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Figure  12.  Virtual  actor  looki 


at  a  ball  in  its  hand. 


Figure  13.  Drinking  actor. 
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Abstract 

To  make  more  complex  interactive  virtual  environments,  it  is  desirable  to  incorporate 
human  figure  models  that  are  autonomous  and  capable  of  independent  and  adaptive 
behavior.  In  order  to  achieve  task-level  interaction  with  virtual  actors,  it  is  abso¬ 
lutely  necessary  to  model  elementary  human  motor  skills.  This  thesis  presents  the 
development  of  the  SkillBuilder,  a  software  system  for  constructing  a  set  of  motor 
behaviors  for  a  virtual  actor  by  designing  motor  programs  for  arbitrarily  complicated 
skills.  It  shows  how  to  generalize  the  generation  of  motor  programs  using  finite  state 
machines.  Visually  guided  reaching,  grasping,  and  head/eye  tracking  motions,  for  a 
kinematically  simulated  actor  have  been  implemented.  All  of  these  actions  have  been 
successfully  demonstrated  in  real-time  by  permitting  the  user  to  interact  with  the 
virtual  environment  using  a  VPL  data  glove. 
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Chapter  1 
Introduction 


As  applications  of  virtual  environments  increase  [1]  it  becomes  more  and  more  impor¬ 
tant  to  incorporate  computer  models  of  human  figures  that  can  move  and  function 
in  such  a  computer  simulated  world.  Especially  for  applications  in  the  education 
and  training  domain,  the  benefit  for  the  virtual  environment  participant  of  interact¬ 
ing  with  human  figure  models,  often  referred  to  as  virtual  actors ,  becomes  obvious. 
There  are  many  situations  in  which  it  would  be,  for  example,  too  hazardous,  too 
distant,  or  too  expensive  to  train  in  the  real  environment.  Moreover,  virtual  actors 
in  a  virtual  environment  can  substitute  for  other  crew  members  to  simulate  team 
training. 

Two  types  of  virtual  actors  can  be  distinguished:  The  guided  and  the  autonomous 
actor.  An  actor  is  guided  if  it  is  slaved  to  the  motions  of  a  human  virtual  environment 
participant.  This  can  be  done,  for  example,  by  means  of  body  tracking.  In  my  thesis 
I  concentrate  on  the  second  kind:  the  autonomous  actor  that  operates  under  program 
control  and  is  capable  of  independent  and  adaptive  behavior. 

Zeltzer  [31]  recognized  the  need  for  finding  the  right  level  of  abstraction  to  control 
simulated  objects  and  articulated  figures  in  virtual  environments.  Considering  that 
the  human  body  consists  of  about  200  degrees  of  freedom,  it  would  certainly  be  much 
too  demanding  to  ask  the  user  to  specify  values  for  all  of  them  in  order  to  control  a 
movement.  It  is  desirable  to  have  a  higher  level  of  interaction  such  that  the  user  need 
only  to  specify  the  goal  of  an  action  rather  than  all  the  necessary  actions  themselves. 
One  big  step  towards  task  level  interaction  is  to  provide  the  virtual  actor  with  a  set 
of  skills  each  of  which  is  represented  by  an  appropriate  motor  program. 

This  thesis  presents  a  software  system,  the  SkillBuilder,  that  can  function  as  a  design 
tool  for  motor  programs.  Motor  programs  are  the  source  code  of  skills  a  virtual  actor 
can  perform.  A  motor  goal  parser  will  take  care  of  translating  a  given  task  from  a 
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natural  language  interface  to  a  set  of  task  primitives  which  can  then  be  passed  to 
the  skill  network.  The  skill  network  will  be  responsible  for  finding  out  which  skills 
need  to  be  executed  in  what  order  and  for  invoking  the  appropriate  motor  programs 
corresponding  to  those  skills.  This  will  allow  the  user  to  interact  with  the  actor  on  a 
task  level. 

The  objective  of  this  thesis  was  to  develop  motor  skills  to  model  an  actors  behavior 
and  to  understand  what  is  needed  to  build  a  visual  programming  environment  for 
defining  motor  skills.  With  regard  to  the  latter,  finite  state  machines  have  been 
chosen  to  represent  motor  skills.  In  order  to  allow  the  user  to  build  a  finite  state 
machine  it  is  essential  to  characterize  the  necessary  components  of  which  a  finite 
state  machine  consists.  First,  the  desired  action  of  each  state  has  to  be  identified  by 
assigning  appropriate  local  motor  programs  and  second,  initial  conditions  as  well  as 
transition  conditions  between  the  states  (also  called  ending  conditions),  have  to  be 
defined.  Furthermore,  it  is  necessary  to  deal  with  issues  like  inverse  kinematics  and 
collision  avoidance. 

The  main  contributions  of  this  thesis  are  the  generalization  of  the  design  of  motor 
programs  and  the  characterization  of  transition  conditions.  A  motor  program  library 
for  interactive  reaching  and  grasping  skills  has  been  created.  It  has  been  demonstrated 
that  the  presented  approach  is  capable  of  generating  approximate  human  movements 
that  look  reasonably  realistic.  The  validation  of  these  movements  with  experimental 
data  is  left  for  future  work. 

After  reviewing  some  of  the  important  work  related  to  the  thesis  topic  in  chapter  2, 
chapter  3  presents  an  overview  of  the  larger  project  in  which  the  SkillBuilder  devel¬ 
opment  was  embedded. 

The  implementation  of  the  SkillBuilder  using  the  virtual  environment  system  3d  will 
be  discussed  in  full  detail  in  chapter  4.  It  encompasses  the  generation  of  a  kine¬ 
matically  controlled  human  figure  model  (section  4.3)  and  explains  how  to  make  it 
move.  Most  basic  movements  are  initiated  by  local  motor  programs  (section  4.4.2) 
that  articulate  a  single  joint  each.  A  lot  of  motor  skills  are  based  on  inverse  kinemat¬ 
ics  algorithms  explained  in  section  4.5.  In  section  4.6,  a  simple  collision  avoidance 
feature  will  be  presented  for  reaching  an  object  placed  on  a  table.  The  general  design 
of  motor  programs  for  different  kinds  of  skills  will  then  be  described  in  section  4.7.  A 
discussion  of  condition  procedures  and  their  characterization  is  included  in  section  4.8. 

The  chapter  about  visually  guided  motion  (chapter  5)  introduces  the  implemented 
skills  that  mainly  include  reaching  and  grasping  motions.  The  thesis  will  close  with 
suggestions  for  future  work  in  chapter  6  and  a  discussion  of  the  results  in  the  conclu¬ 
sion  (chapter  7). 


Chapt  er  2 
Related  Work 


Some  issues  in  related  fields,  i.e.  robotics,  motor  behavior,  and  figure  animation,  have 
been  reviewed  during  the  development  process  of  the  SkillBuilder. 

The  control  of  a  human  figure  is  closely  related  to  robotics  given  that  the  figure  model 
can  be  viewed  as  a  composition  of  several  robot  manipulators.  Paul  describes  the 
Denavit-Hartenberg  joint  notation  [19]  that  is  used  in  the  SkillBuilder  to  construct 
the  kinematic  chains  for  the  extremities.  Lozano-Perez  [14]  and  Brooks  [5]  have 
developed  motion  planning  algorithms  that  will  be  of  very  much  use  at  a  later  state 
of  the  SkillBuilder. 

Researchers  in  motor  behavior  have  studied  the  underlying  processes  of  movement 
control  in  humans  and  come  to  the  conclusion  that  a  sequence  of  movements  can  be 
coordinated  in  advance  of  their  execution  to  form  a  single  complex  action  [4]  [25] 
[23].  They  call  this  process  motor  planning  or  motor  programming.  Bizzi  [4]  studied 
visually  triggered  head  and  arm  movements  in  monkeys  under  the  assumption  that 
the  arm  movement  controlling  motor  commands  are  precomputed  somewhere  in  the 
central  nervous  system  prior  to  movement  initiation  and  that  muscles  can  be  modeled 
by  a  mass/spring  system  including  damping.  He  found  that  the  motor  program 
specifies  an  intended  equilibrium  point  between  sets  of  agonist  and  antagonist  muscles 
that  correctly  positions  the  arm  and  the  head  in  relation  to  a  virtual  target.  However, 
motor  programs  in  the  SkillBuilder  control  the  movements  by  specifying  necessary 
joint  angle  changes. 

In  [25]  Schmidt  discusses  the  nature  of  motor  programs  and  their  interaction  with 
peripheral  feedback.  According  to  him,  motor  responses  can  be  characterized  in  two 
kinds  of  movements:  1)  very  short  movements  (<  200msec)  that  are  planned  in  ad¬ 
vance  and  do  not  seem  to  be  consciously  controllable  while  executed  and  2)  longer 
movements  with  the  opportunity  for  conscious  control  via  feedback  during  the  move- 
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ment.  His  considerations  are  important  for  the  SkillBuilder  to  decide  when  to  allow 
interaction  while  executing  a  motor  program  and  how  big  a  motor  program  unit 
should  be.  Schmidt  uses  the  term  motor  program  as  an  abstract  memory  structure 
that  is  prepared  in  advance  of  the  movement  causing  muscle  contractions  and  relax¬ 
ations  when  executing.  Furthermore,  he  compares  the  feedback  during  the  execution 
of  motor  programs  to  if-statements  in  computer  programs.  This  suggests  that  using 
finite  state  machines  is  a  good  choice  to  model  motor  programs  in  the  SkillBuilder. 

Shaffer  analyses  the  basic  properties  of  skilled  performances  [23].  He  identifies  fluency, 
expressiveness  and  flexibility  as  basic  properties  of  a  skilled  piano  performance. 

Badler  and  his  research  group  at  the  University  of  Pennsylvania  have  been  work¬ 
ing  on  figure  animation  for  several  years.  They  developed  the  Jack  system  [3]  for 
the  definition,  manipulation,  animation,  and  human  factors  performance  analyses 
of  simulated  human  figures  which  is  most  comparable  to  the  SkillBuilder.  Like  in 
the  current  state  of  the  SkillBuilder  they  were  most  concerned  in  capturing  some  of 
the  global  characteristics  of  human-like  movements  without  modeling  too  deeply  the 
physical  laws  of  nature,  so  they  describe  motions  kinematically.  Their  figures  consist 
of  rigid  body  segments  connected  by  joints  like  the  SkillBuilder  actor.  They  spent 
a  lot  of  effort  in  articulating  the  torso  [20]  by  providing  18  vertebral  joints  whereas 
the  SkillBuilder  concentrates  more  on  the  articulation  of  the  hand.  Jack  contains 
features  for  anthropometric  figure  generation,  end  effector  positioning  and  orienting 
under  constraints,  real-time  end  effector  dragging,  rotation  propagation  when  joint 
limits  are  exceeded,  and  strength  guided  motion.  However,  to  animate  a  motion  se¬ 
quence  they  use  keyframing  of  a  series  of  postures  and  the  subsequent  interpolation 
of  the  joint  angles  for  postures  in  between.  The  SkillBuilder  in  contrast,  computes 
the  joint  angles  for.  the  posture  to  be  rendered  at  the  next  time  frame  in  real-time. 
The  animation  sequences  of  Jack  are  primarily  scripted  and  not  provoked  by  motor 
programs  which  is  what  the  SkillBuilder  focuses  on.  Jack  does  not  provide  any  tool 
to  define  motor  skills.  Badlers  et  al  ultimate  goal  is  to  provide  a  high  level  of  task 
understanding  and  planning.  Their  approach  includes  research  in  natural  language 
instructions  for  driving  animated  simulations.  The  SkillBuilder  is  part  of  a  bigger 
project  that  also  includes  research  on  natural  language  to  describe  tasks,  as  will  be 
discussed  in  chapter  3. 

Dynamics  simulations  as  Wilhelms  proposes  in  [2S]  allow  physically  correct  motions 
by  using  forces  and  torques  to  drive  a  figure.  Finding  realistic  movement  laws  is  a 
difficult  undertaking  which  often  leads  to  movements  that  seem  too  regular.  Besides, 
solving  the  equations  of  motion  is  computationally  very  expensive  and  not  necessarily 
needed  at  this  state  of  the  SkillBuilder  since  the  concern  was  not  to  model  motions 
most  realistically,  but  find  a  means  by  what  to  describe  motor  behavior. 
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Girard  also  argues  [12]  that  the  torque  functions  needed  at  each  joint  to  produce 
a  desired  limb  motion  are  sufficient  to  capture  uncoordinated  limb  motion  but  that 
goal-oriented  behavior  cannot  be  properly  simulated  without  kinematic  trajectory 
planning.  He  has  been  working  on  inverse  kinematics  and  visual  programming  in¬ 
terfaces  to  describe  motor  behavior  for  a  number  of  years  and  concentrated  mostly 
on  locomotion  and  dance.  His  figures  can  be  controlled  interactively  by  specifying  a 
spline  curve  as  path  for  an  end  effector  and  applying  inverse  kinematics  to  compute 
the  desired  joint  angles  of  a  limb.  Rotations  of  joints  can  also  be  specified  directly  to 
achieve  desired  limb  postures.  Assembling  several  postures  yields  a  working  posture 
sequence.  Yet  a  third  method  is  to  adjust  a  posture  while  keeping  the  end  effector 
at  its  current  location.  The  SkillBuilder  incorporates  all  of  the  named  possibilities 
in  its  inverse  and  forward  kinematics  skills.  Posture  changes  for  a  fixed  end  effector 
position  can  be  achieved  by  modifying  a  global  posture  parameter  for  a  limb. 

Investigations  on  knowledge-based  human  grasping  have  been  done  by  Rijpkema  and 
Girard  [22].  Their  hand  model  is,  very  similar  to  the  SkillBuilder  hand,  kinematically 
articulated  by  using  the  Denavit-Hartenberg  notation.  The  high  level  control  they 
propose  for  the  hand  includes  single-finger  control,  group  control  (closing  and  opening 
or  spreading  a  group  of  fingers),  and  hand  control  (using  a  hand  posture  library).  All 
the  named  methods  are  incorporated  in  the  SkillBuilder. 

Calvert  describes  the  ideal  human  figure  animation  system  [2]  as  a  system  that  should 
accept  natural  language  and  graphical  input  at  its  highest  level  and  generate  a  script 
with  the  details  of  movement  specifications.  The  lowest  level  would  comprise  detailed 
movement  instructions  for  each  limb  segment  as  a  function  of  time.  He  acknowledges 
that  they  are  still  far  away  from  its  realization.  He  and  his  research  group  devel¬ 
oped  a  prototype  system,  COMPOSE,  for  outlining  a  sequence  for  multiple  animated 
dancers.  Many  different  stance  phases  for  the  dancers  can  be  defined  for  placement 
in  a  number  of  scenes  much  like  storyboard  sketches. 

A  motion  control  system,  PINOCCHIO,  is  described  by  Maiocchi  and  Pernici  [16]. 
They  classify  movements  and  their  attributes  using  entries  from  natural  language  in  a 
general  movement  dictionary.  Motions  from  their  motion  database  can  be  assembled 
in  an  animation  script  to  display  a  movement  sequence.  Since  the  movements  are 
recorded  from  real  human  motions,  however,  no  real-time  interaction  with  the  system 
is  possible. 

Magnenat-Thalmann  and  Thalmann  present  a  body  animation  system  based  on  para¬ 
metric  keyframing  [15].  In  very  tedious  sessions  the  animator  has  to  specify  parame¬ 
ters  like  joint  angles  of  a  stick  figure  model  to  describe  frame  by  frame.  Without  the 
possibility  of  any  further  interaction,  in  between  frames  will  be  interpolated  and  the 
HUMAN  FACTORY  software  will  transform  the  character  accordingly  while  mapping 
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surfaces  onto  the  stick  figure.  Joint  dependent  local  deformation  operators  control 
the  evolution  of  surfaces  to  model  soft  tissue.  Since  the  system  generates  relatively 
realistic  looking  images  but  does  not  allow  user  interaction  (either  in  real-time  or  on 
a  sufficiently  high  level),  it  is  more  suitable  for  making  movies  than  for  the  purposes 
aimed  for  by  the  SkillBuilder. 

Zeltzer,  who  is  involved  in  the  project  that  includes  the  SkillBuilder  development,  has 
been  working  for  many  years  in  the  field  of  figure  animation.  He  describes  the  layers  of 
abstraction  for  representing  and  controlling  simulated  objects  and  agents  at  different 
interaction  levels  [31].  At  Ohio  State  University  he  had  developed  the  walking  skeleton 
George  that  is  based  on  a  skeleton  description  language.  George’s  different  gaits  and 
jumps  are  controlled  by  motor  programs  using  finite  state  machines  [29].  In  recent 
years,  he  and  Johnson  have  concentrated  on  the  process  of  motor  planning  [32]. 
Zeltzer  [33]  accounts  for  everyday  human  activities  by  introducing  task  primitives 
and  reports  motor  goal  parsing  as  a  means  to  parse  restricted  natural  language  input 
into  motor  skills.  The  motor  goal  parser  will  be  explained  in  section  3.3  of  this  thesis. 


Chapter  3 

Architecture  of  the  SkillBuilder 


3.1  Task  Level  Interaction  with  Virtual  Actors 


The  development  of  the  SkillBuilder  is  part  of  a  project  called  Virtual  Sailor  which 
has  as  its  goal  the  implementation  of  a  virtual  environment  that  includes  autonomous 
and  interactive  virtual  actors.  The  attempt  is  to  provide  a  level  of  interaction  with 
which  the  end-user  is  familiar  from  everyday  life,  i.e.  through  language  and  gesture  in 
real  time.  To  achieve  this  kind  of  interaction  that  Zeltzer  calls  task  level  interaction 
[31],  elementary  human  motor  skills  have  to  be  modeled  and  integrated  into  a  be¬ 
havior  repertoire  for  an  autonomous  actor.  Furthermore,  a  task-level,  language-based 
interface  has  to  be  developed. 

The  presented  research  is  particularly  concerned  with  the  kinds  of  mechanical  oper¬ 
ations  on  physical  objects  encountered  in  everyday  activities,  such  as  reaching  and 
grasping,  and  manipulating  tools  and  commonplace  objects.  The  current  effort  is 
focussed  on  the  simulation  of  one  individual  actor.  However,  the  ultimate  goal  is  to 
have  multiple  actors  that  are  capable  of  coordinating  their  behavior  with  the  actions 
of  other  actors  or  human  participants  in  the  virtual  environment  [35]. 

The  SkillBuilder  is  a  tool  for  constructing  a  set  of  behaviors  for  a  virtual  actor  by 
designing  motor  programs  for  arbitrarily  complicated  skills.  The  coordination  of  the 
skills  to  perform  a  given  task  has  to  be  undertaken  at  runtime  by  a  separate  mech¬ 
anism,  i.e.  the  motor  planning  system,  which  takes  care  of  selecting  and  sequencing 
motor  skills  appropriate  to  the  actor’s  behavioral  goals  and  the  state  of  objects. 
Zeltzer  and  Johnson  call  motor  planning  the  process  of  linking  perception  of  objects 
and  events  with  action  [32]. 

Finally,  an  interface  for  constrained  natural  language  input  is  needed  to  “translate” 
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a  task  description,  e.g.  “Go  to  the  desk  and  pick  up  the  glass”  into  a  set  of  task 
primitives  [34].  The  motor  goal  parser  currently  under  development  by  Zeltzer  takes 
care  of  this  decomposition. 


3.2  Waves  World  and  the  Skill  Network 

Johnson  is  working  on  the  development  of  Waves  World,  a  software  system  imple¬ 
mented  for  designing,  building  and  debugging  the  object  database,  distributed  virtual 
environments,  and  virtual  actors,  of  which  a  detailed  description  can  be  found  in  [34]. 

The  motor  planner  component  of  Waves  World,  a  reactive  planning  algorithm,  is  based 
on  a  skill  network  [30],  [32],  [13]  to  model  the  behavior  of  virtual  actors.  A  collection  of 
motor  skills  can  be  assembled  into  the  skill  network  and  the  output  can  be  visualized 
by  using  Waves  World.  The  execution  of  motor  acts  depends  on  sensory  input  and 
current  behavioral  goals. 

A  virtual  actor’s  skill  network  is  comprised  of  several  agents  that  are  each  realized  as 
a  set  of  processes  distributed  over  a  network  of  workstations.  There  are  skill  agents, 
sensor  agents,  goal  agents  and  a  registry/dispatcher. 

The  sensor  agents  handle  the  perception  of  a  virtual  actor  by  measuring  signs  and 
signals  from  the  virtual  world  represented  by  proposition-value  pairs,  e.g.  proposi¬ 
tion:  a-door-is-nearby,  value:  TRUE.  Goals  are  handled  by  goal  agents  that  are 
represented  as  desired  states,  i.e.  a  proposition-value  pair  that  needs  to  be  sensed 
true  by  a  sensor  agent  to  satisfy  the  goal.  Finally,  skill  agents  control  the  behavior  of 
a  virtual  actor  and  attach  a  set  of  pre-  and  postconditions  to  a  skill  name.  A  skill  can 
only  be  selected  by  the  planning  algorithm  for  execution  if  all  its  preconditions  are 
fulfilled.  Postconditions  are  predictions  about  the  state  of  the  virtual  world  after  the 
execution  of  the  skill.  The  central  communication  among  all  these  agents  is  handled 
by  the  registry/dispatcher  which  maintains  a  shared  database  for  all  agents. 

The  skill  network  is  characterized  by: 

•  a  comprehensive  sensing  structure 

•  high-level  controls  through  time-varying  sampling  rates 

•  computational  economics  at  the  lowest  level 

•  direct-manipulation  interface  to  a  large  set  of  heterogeneous  computing  re¬ 
sources. 


3.3  Motor  Goal  Parser 
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3.3  Motor  Goal  Parser 


This  section  largely  borrows  from  [34]  in  which  Zeltzer  describes  the  process  of  motor 
goal  parsing. 

In  motor  goal  parsing  (MGP),  the  task  manager  must  decompose  each  task  descrip¬ 
tion  into  a  set  of  well-defined  motor  units,  i.e.  task  primitives,  that  can  be  named 
and  simulated.  A  task  description  is  a  constrained  natural  language  description  of  a 
specific  movement  sequence,  e.g.  “Go  to  the  door  and  close  it”.  The  set  of  task  prim¬ 
itives  that  corresponds  to  the  everyday  activities  to  be  simulated  need  to  be  specified. 
In  the  above  example  they  might  include  “move.to(door)”,  “grasp(doorknob)”,  and 
“rotate(doorknob)” . 

For  two  reasons,  however,  the  mapping  from  these  task  primitives  to  specific  mo¬ 
tor  skills  cannot  be  one-to-one  a  priori,  and  the  task  primitives  must  be  effector- 
independent  at  the  conceptual  level.  First,  because  of  the  problem  of  motor  equiva¬ 
lence:  a  given  goal  can  often  be  accomplished  in  various  ways,  e.g.  the  door  can  be 
closed  by  turning  the  doorknob  with  the  right  or  left  hand  or  by  pushing  the  door 
with  one  of  the  feet.  Second,  the  objects  named  in  the  task  description  may  deter¬ 
mine  which  skill  or  skills  are  necessary  to  accomplish  the  task.  If,  instead  of  “Go 
to  the  door”,  the  command  were  “Go  to  Mexico”,  the  task  manager  is  expected  to 
output  a  very  different  list  of  skills.  Once  the  task  primitives  have  been  identified 
from  the  input  task  description,  the  task  manager  must  consider  the  states  of  the 
virtual  actor,  the  targeted  objects,  and  ongoing  events  in  the  virtual  world,  and  then 
determine  the  appropriate  effector  systems  and  motor  skills  to  invoke.  Therefore,  the 
task  primitives  must  constitute  an  intermediate,  effector-independent  representation 
which  drives  the  selection  of  the  underlying  motor  skills  to  be  performed. 

The  task  primitives  Zeltzer  uses  are  based  on  a  set  of  primitive  “ACTs”  from  Schank’s 
Conceptual  Dependency  (CD)  theory  [24]  and  on  “A-l  action  units”  described  by 
Schwartz  et  al  [26].  Some  of  the  task  primitives  represent  physical  actions,  and  some 
of  them  represent  abstract  “mental  actions”. 

Zeltzer  defines  atomic  skills  as  such  that  serve  as  task  primitives  while  composite 
skills  represent  learned  behaviors.  He  proposes  that  affordances,  (i.e.  the  properties 
of  objects  and  their  environment  that  are  necessary  predictions  for  executing  partic¬ 
ular  acts),  should  be  stored  with  the  task  primitives.  He  argues  that  the  notion  of 
affordances  suggests  the  right  approach  for  representing  the  common  sense  knowledge 
that  makes  it  possible  to  perform  everyday  activities. 

An  example  of  a  motor  goal  parsing  process  is  given  in  [34]. 
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3.4  Components  of  the  SkillBuilder 

The  SkillBuilder  consists  of  several  components  as  shown  in  figure  3-1  [35]: 


•  A  skill  template  that  has  to  be  filled  in  by  the  user  with  all  necessary  information 
for  the  motor  planner:  a  set  of  pre-  and  postconditions,  the  end  effector  involved 
in  the  skill,  and  a  pointer  to  the  appropriate  motor  program. 

•  A  motor  program  builder  that  generates  motor  programs:  the  source  code  for 
an  actor’s  skill  based  on  finite  state  machines,  i.e.  definite  states  in  if-then-else 
clauses. 

•  A  motion  checker  module  that  serves  to  display  and  visualize  the  execution  of 
defined  skills. 


Figure  3-1:  Block  diagram  of  the  SkillBuilder 


3.4  Components  of  the  SkillBuilder 
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The  main  focus  of  the  presented  work  is  put  in  the  understanding  of  the  design 
of  motor  programs  and  the  elaboration  of  a  general  structure  for  motor  programs  by 
developing  motor  programs  in  particular  for  reaching  and  grasping.  The  development 
environment  3d  (to  be  explained  in  section  4.1)  provides  all  necessary  tools  to  display 
the  defined  skills  which  is  the  task  of  the  motion  checker  module. 


3.4.1  Model  of  the  Virtual  Actor 

The  development  of  the  SkillBuilder  includes  the  generation  of  a  virtual  actor,  a  com¬ 
puter  model  of  a  human  figure  that  consists  of  a  compound  of  rigid  body  parts  linked 
together  by  a  kinematic  structure.  The  model  has  to  be  simple  enough  to  allow  real¬ 
time  interaction  and  facilitate  the  software  development.  Furthermore,  it  is  not  the 
intent  to  build  the  most  realistic  looking  virtual  actor  but  rather  have  an  articulated 
model  that  suffices  to  show  the  execution  of  skills  in  order  to  learn  how  to  generalize 
the  design  of  motor  programs.  A  rigid  body  model  purely  animated  by  kinematics 
will  serve  this  purpose  for  the  time  being.  As  fidelity  requirements  increase  for  specific 
applications  the  kinematic  model  can  later  be  extended  to  simulate  dynamics. 


3.4.2  Motor  Program  Design 

Every  skill  an  actor  can  perform  is  based  on  a  motor  program  that  coordinates  the 
execution  of  several  local  motor  programs  and  is  modeled  by  a  finite  state  machine.  A 
local  motor  programs  is  the  mechanism  that  controls  the  rotation  about  a  single  joint 
axis.  The  use  of  finite  state  machines  to  control  walking  skills  has  been  successfully 
demonstrated  by  Zeltzer  [29],  Raibert  [21],  and  Brooks  [6]. 

A  finite  state  machine  to  model  an  actor’s  skill  can  consist  of  any  number  of  branches 
and  feedback  loops  and  can  be  arbitrarily  complicated  (figure  3-2).  Furthermore, 
oscillators,  servos,  reflexes,  mass/spring  systems  and  collision  detection  module  all 
can  be  realized  by  finite  state  machines. 

Most  important  is  to  decide  which  local  motor  programs  should  be  involved  in  the 
performance  of  the  skill.  Often,  a  skill  can  be  modeled  in  many  different  ways, 
involving  different  local  motor  programs.  The  next  step  is  to  decide  which  local 
motor  programs  make  up  each  state  and  how  the  designed  states  should  be  connected. 
Finally,  the  initial  conditions  for  the  skill  and  the  ending  conditions  of  each  state  have 
to  be  defined. 

Each  state  is  executed  until  its  ending  conditions  are  fulfilled.  Ending  conditions  can 
be  geometric  constraints  (“Do  this  until  the  end  effector  is  closer  to  a  specified  point 
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Figure  3-2:  Example  finite  state  machine  to  model  an  actor’s  skill 


than  a  given  goal  point  precision ”),  joint  angle  constraints  (increase/decrease  a  joint 
angle  until  it  reaches  a  specified  target  joint  angle  or  its  limits),  or  even  external 
constraints  (“Stop  executing  as  soon  as  another  motor  program’s  ending  conditions 
are  fulfilled”).  Once  the  ending  conditions  for  a  motor  program  are  satisfied,  its  end 
event  will  be  evaluated.  In  defining  another  motor  program  as  end  event,  several 
motor  programs  can  be  linked  to  execute  sequentially.  Motor  programs  for  atomic 
skills  will  be  classified  as  follows: 

1.  Forward  kinematics  skills 

Forward  kinematics  skills  are  modeled  by  finite  state  machines  with  usually 
more  than  one  state.  Examples  of  implemented  forward  kinematics  skills  are 
put_arra_on_table  and  reach_ceiling. 

2.  Inverse  kinematics  skills 

Inverse  kinematics  skills  usually  have  just  one  state  and  contain  an  inverse  kine¬ 
matics  calculation  procedure  to  synchronize  the  angle  step  size  for  all  local  motor 
programs  involved.  Examples  of  inverse  kinematics  skills  are  move_arm_to_goal 
and  direct_hand. 

Motor  programs  for  composite  skills  are  composed  of  several  atomic  skills  that  will 
execute  in  parallel,  sequentially,  or  in  any  possible  combination  of  those  two.  This 
will  be  explained  in  more  detail  in  section  4.7.3. 


3.4  Components  of  the  SkillBuilder 
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3.4.3  The  Ultimate  SkillBuilder  Interface 

The  ultimate  SkillBuilder  should  provide  a  visual  programming  language  for  describ¬ 
ing  motor  skills.  Through  the  use  of  a  graphical  interface  the  user  should  be  able  to 
point  and  click  on  selections  of  initial  conditions,  as  well  as  selections  of  local  motor 
programs,  defining  each  state  of  a  finite  state  machine  for  an  atomic  skill.  He  should 
then  be  able  to  click  and  drag  on  symbols  for  each  state  to  position  them  in  the  order 
of  his  choice,  and  then  draw  pointers  between  those  states  to  define  their  connections. 

A  second  graphical  module  should  provide  a  menu  of  all  pre-defined  atomic  skills, 
from  which  the  user  could  select  as  many  as  he  wants  and  then  assemble  them  in  an 
arbitrarily  complicated  network,  thereby  building  a  composite  skill. 

For  atomic,  as  well  as  for  composite  skills,  there  should  be  an  appropriate  motor 
program  builder  to  automate  the  generation  of  the  motor  program  procedures  that 
make  up  a  skill  once  it  has  been  graphically  set  up  by  the  user. 

A  simple  click  on  a  motion  checker  button  should  execute  the  newly  defined  skill  to 
see  whether  the  simulation  satisfies  the  expected  performance. 
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Chapter  4 

Implementation 


4.1  Development  Environment  for  the  SkillBuilder 

4.1.1  The  3d  Virtual  Environment  Simulation  System 

The  SkillBuilder  was  developed  using  the  virtual  environment  system  3d ,  an  inter¬ 
pretive  toolkit  for  creating  custom  VE  applications. 

3d  is  a  “testbed”  system  designed  to  support  the  specification  of  behaviors  for  virtual 
worlds.  It  has  been  developed  by  the  Computer  Graphics  and  Animation  Group  of 
M.I.T.’s  Media  Lab.  Systematic  descriptions  of  3d  can  be  found  in  [8]  and  [7].  The 
system  provides  an  interpreted  command  language  that  has  many  special  purpose 
rendering,  dynamics,  numerical  math  and  user-interface  functions. 

The  syntax  for  3d  is  based  on  the  “tool  command  language”  ( tel)  from  U.C.  Berkeley 
[17].  The  tel  environment  includes  an  interpreted  command  evaluator  that  allows 
rapid  prototyping  of  virtual  worlds  and  VE  interfaces.  The  application-specific  code 
for  3d  has  been  developed  on  top  of  the  RenderMatic  C  library  of  graphics  and  ren¬ 
dering  software  [10].  The  command  interpreter  has  over  700  built-in  and  application- 
specific  functions.  There  are  primitives  for  scene  description  and  rendering,  math,  ma¬ 
trix  and  vector  operations,  general  data  structure  manipulation,  Denavit-Hartenberg 
joint  description,  finite  element  dynamics,  and  X/Motif  interface  building.  A  list  of 
all  available  3d  commands  can  be  found  in  the  appendix  of  [7]. 

In  3d  it  is  very  easy  to  build  a  collection  of  useful  subroutines,  because  tel  allows  pro¬ 
cedure  definition.  In  this  way,  the  application  software  can  be  organized  by  creating 
interpreted  layers  of  language,  as  has  been  done  in  the  work  presented. 
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4.1.2  The  Tel  Embeddable  Command  Language 

For  a  better  understanding  of  the  software  examples  in  this  thesis,  a  brief  description 
of  the  tel  language  syntax  follows.  This  section  borrows  largely  from  [18]. 

The  syntax  of  tel  is  similar  to  the  UNIX  shell  syntax:  A  command  consists  of  one  or 
more  fields  separated  by  blanks.  The  first  field  is  the  name  of  the  command,  which 
can  be  either  a  build-in  or  application-specific  command,  or  a  tel  proc  constructed 
from  a  sequence  of  other  commands.  Subsequent  fields  are  passed  to  the  command 
as  arguments.  Newlines  and  semicolons  separate  commands.  Tel  commands  return 
a  string  according  to  their  evaluation,  or  an  empty  string  if  no  return  value  was 
specified. 

Additional  constructs  give  tel  a  lisp-like  feel.  Comments  are  delineated  by  a  pound 
sign  (#).  The  backslash  character  (\)  denotes  line  continuation,  or  escapes  special 
characters  for  insertion  as  arguments.  Curly  braces  ({})  can  be  used  to  group  complex 
list  arguments.  For  example,  the  command 

set  echs  {molchi  {hoch  drei}} 

has  two  arguments,  “echs”,  and  “molchi  {hoch  drei}”.  The  set  command  sets  the 
first  argument  to  be  the  value  of  the  second  argument. 

Command  substitution  is  invoked  by  square  brackets  ([]).  The  content  between  those 
brackets  is  treated  as  a  command  and  evaluated  recursively  by  the  interpreter.  The 
result  of  the  command  is  then  substituted  as  the  argument  in  place  of  the  original 
square  bracketed  string  so  that 

div  6  [minus  4  l] 

for  example  returns  2  because  “minus  4  1”  returns  3. 

Finally,  the  dollar  sign  ($)  is  used  for  variable  substitution.  If  the  dollar  appears  in 
an  argument  string,  the  subsequent  characters  are  treated  as  a  variable  name,  and 
the  contents  of  the  variable  are  then  substituted  as  the  argument,  in  the  place  of  the 
dollar  sign  and  variable  name.  For  example, 

set  x  [sqrt  4] 
plus  5  $x 


returns  7  because  the  variable  x  has  the  value  2. 


4.1  Development  Environment  for  the  SkillBuilder 
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4.1.3  The  Denavit-Hartenberg  Notation 

To  support  the  construction  of,  and  interaction  with,  kinematic  chains  as  needed 
for  robotics  applications,  the  Denavit-Hartenberg  joint  notation  [19]  has  been  imple¬ 
mented  in  3d.  This  feature  has  been  used  in  the  presented  work  to  build  the  structure 
of  a  jointed  figure  by  defining  kinematic  chains  for  each  of  the  extremities. 

The  Denavit-Hartenberg  notation  is  a  systematic  method  of  describing  the  kinematic 
relationship  between  a  pair  of  adjacent  links  with  a  minimum  number  of  parameters. 
The  relative  position  and  orientation  of  the  two  coordinate  frames  attached  to  those 
two  links  can  be  completely  determined  by  the  following  4  parameters  (denoted  as 
dhparameters,  see  figure  4-1)  : 

1.  an  :  the  length  of  the  common  normal 

2.  dn  :  the  distance  between  the  origin  of  joint  n  and  the  common  normal  along 

zn+l 

3.  an  :  the  twist  angle  between  the  joint  axes  about  xn 

4.  0n  :  the  angle  between  the  xn_i  axis  and  the  common  normal  measured  about 
Zn— 1 


In  a  human  skeleton  all  degrees  of  freedom  can  be  satisfactorily  modeled  by  revolute 
joints.  Changing  the  angle  0n  will  cause  a  link  n  to  turn  about  the  zn_!  axis.  All  other 
dhparameters  will  remain  constant.  The  relationship  between  adjacent  coordinate 
frames  can  be  expressed  in  a  4x4  matrix: 


(  cos  0,-  —  sin  0,-  cos  a,- 
sin  0,-  cos  0,-  cos  a,- 
0  sin  a,- 

\  0  0 


sin  0,  sin  a,-  a,-  cos  0,-  \ 

—  cos  0;  sin  a,-  a,sin0, 
cos  a,-  d{ 

0  1  / 


(4.1) 


Thus,  the  position  and  rotation  matrix  of  joint  n  can  be  calculated  by  multiplying  the 
position  and  rotation  matrix  of  a  joint  m  (n  >  m)  with  all  transformation  matrices 
from  each  joint  in  between  m  and  n  to  its  adjacent  joint: 

Mn  =  Mm  •  Am+i  •  Am+2  • ...  •  Au_i  •  Au  ,  (n  >  m)  (4.2) 

In  3d  every  defined  Denavit-Hartenberg  joint  n  has  a  dlimatrix  assigned  to  it  which 
is  the  matrix  Mn  from  equation  (4.2).  This  way,  3d  internally  takes  care  of  providing 
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Joint  n  +  l 


the  absolute  transformation  matrices  for  all  body  parts,  depending  on  the  value  of 
the  dhparameters. 

Spherical  joints  can  be  defined  by  a  system  of  three  coincident  joints,  where  no  two 
joint  axes  are  parallel.  When  defining  the  kinematic  chains  for  the  extremities,  a 
few  “dummy”  joints  have  to  be  introduced.  This  is  caused  by  the  fact  that  it  is  not 
always  possible  to  rotate  the  subsequent  coordinate  frame  to  line  up  as  desired,  since 
the  twist  angle  a  only  produces  a  rotation  about  xn.  The  “dummy”  joints  will  not 
move  but  only  serve  to  modify  the  coordinate  frame  orientation. 


4.2  Graphical  User  Interface 

The  user  interface  of  3d  consists  of  four  kinds  of  dialogs: 

1.  Keyboard  input 

2.  Manipulators  (such  as  glovetool  to  control  a  data  glove) 

3.  Mouse  manipulation  of  objects  inside  the  graphics  window 
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4.  Interactive  X  Dialog  Boxes 

3d  supports  building  of  graphical  interfaces  with  application-specific  commands  [7] 
imported  from  OSF/Motif™  widgets  [27]. 

It  is  desirable  to  control  as  much  as  possible  through  the  use  of  a  graphical  interface 
in  order  to  make  the  SkillBuilder  easier  to  use.  For  the  most  part,  however,  the  actor 
has  to  be  controlled  by  using  the  keyboard,  and  typing  commands  directly  in  the  3d 
window  at  the  3d>  prompt.  The  graphical  user  interface  for  the  SkillBuilder  is  an 
important  area  that  must  be  improved  in  the  future. 

In  the  following  section  the  graphical  user  interface  that  has  been  used  during  the 
development  of  the  SkillBuilder  will  be  explained.  The  3d  Menu  provided  by  Dave 
Chen  [8]  has  been  extended  to  include  a  few  more  features,  and  the  Views  menu 
developed  by  Steven  Drucker  has  been  integrated.  The  Command  Loop  menu  (see 
figure  4-9,  page  47)  is  based  on  the  idea  of  a  command  loop  also  provided  by  Steven 
Drucker  that  has  been  implemented  as  a  graphical  dialog  for  the  SkillBuilder.  It  will 
be  explained  in  section  4.4.3. 

4.2.1  The  Extended  3d  Menu 

Figure  4-2  shows  the  extended  3d  Menu  that  automatically  pops  up  when  calling 
the  SkillBuilder. 

Features  that  were  not  included  in  the  basic  3d  Menu  are: 

1.  Current  Object  as  a  button  providing  a  pull  down  menu  of  all  currently 
available  objects. 

2.  The  Command  text  field. 

What  follows  is  a  review  and  explanation  of  the  most  important  features  used  during 
the  development  of  the  SkillBuilder. 


Changing  the  Current  Object 

Clicking  on  the  Current  Object  button  will  cause  a  pull  down  menu  listing  all 
currently  existing  objects  to  appear.  If  one  of  those  objects  is  chosen,  it  will  appear 
in  the  text  field  right  below  the  Current  Object  button.  The  current  object  can 
also  be  set  by  clicking  on  the  object  in  question  with  the  middle  mouse  button,  or  by 
typing  it  in  the  text  field. 
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Figure  4-2:  Extended  3d  Menu 
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Object  Edit  Popup  Menu 


Figure  4-3:  Object  Edit  popup  menu 


To  make  the  Object  Edit  menu  (see  figure  4-3)  pop  up,  click  the  Object  label  in  the 
upper  left  corner  of  the  3d  Menu,  and  then  choose  Edit  from  the  pull  down  menu 
that  appears.  Toggle  buttons  are  provided  to  choose  the  action  to  be  taken  by  the 
current  object  (whose  name  appears  below  the  Current  Object  button  in  the  3d 
Menu).  The  current  object  can  be  translated,  rotated,  scaled,  or  changed  in  color. 
For  the  first  three  actions,  the  arrow  buttons  in  the  menu  correspond  to  the  x,  y,  and 
z  axis  in  this  order,  whereas  when  changing  the  color,  those  buttons  control  the  r,  g, 
and  b  values  in  the  rgb  code.  The  +  and  the  -  buttons  change  all  three  values  at  the 
same  time  by  the  same  amount.  Choosing  “local”  in  the  Transform  field  makes  the 
centroid  of  the  object  the  reference  frame.  A  different  reference  frame  can  be  chosen 
by  typing  its  three  coordinates  in  this  text  field  instead  of  “local”. 

Posting  and  Unposting  Objects 

Objects  can  be  easily  posted  or  unposted  by  chosing  the  option  Post  or  Unpost  in 
the  pull  down  menu  which  appears  when  clicking  on  Object  in  the  upper  left  corner 
of  the  3d  Menu.  The  object  that  will  be  posted  or  unposted  is  the  current  object. 

Limb  Edit  Popup  Menu 

The  Limb  Edit  menu  (see  figure  4-4)  can  be  invoked  by  clicking  Limb  in  the  upper 
right  corner  of  the  3d  Menu  and  pulling  down  to  Edit.  The  Current  Limb  has  to  be 
set  to  one  of  the  limbs  that  is  defined  as  a  dhchain,  i.e.  Larm,  spine,  Lleg  and  Lshoe. 
Clicking  now  on  one  of  the  arrows  in  the  lower  right  corner  of  the  Limb  Edit  menu 
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will  increase  or  decrease  the  current  joint  number.  A  coordinate  frame  displaying 
the  position  and  orientation  of  the  dhmatrix  of  the  current  joint  will  appear  in  the 
graphics  window.  To  change  the  current  joint  angle,  the  slider  above  the  label  Theta 
can  be  changed.  Since  the  dhparameter  alpha  would  change  the  definition  of  the 
relationship  between  two  links,  its  value  should  not  be  modified  once  the  kinematic 
chain  of  a  limb  has  been  set  up.  Note  that  it  is  possible  to  move  the  joints  to  angles 
outside  of  their  limits.  That  is  the  reason  why  the  Limb  Edit  menu  is  meant  only 
to  be  used  for  testing  and  development  purposes. 


Command  text  field 

At  the  bottom  of  the  3d  Menu  there  is  a  text  field  labeled  Command.  Any  com¬ 
mand  that  might  be  typed  in  at  the  3d>  prompt  in  the  3d  window  can  also  be  typed  in 
here  with  the  same  result.  Once  the  command  loop  is  running  (refer  to  section  4.4.3), 
the  3d  window  does  not  accept  any  input,  but  the  menu  windows  do,  because  of  an 
x  flush  event  at  the  end  of  each  loop  step. 


Using  the  Mouse  in  the  Graphics  Window 

Mouse  use  in  the  graphics  window  to  change  the  view  is  explained  in  detail  in  the 
User  Manual  included  in  appendix  C,  page  141. 

4.2.2  Views  Menu 

With  the  kind  permission  of  Steven  Drucker,  the  Views  menu  (see  figure  4-5)  devel¬ 
oped  by  him  is  made  available  for  use  in  the  SkillBuilder.  The  views  menu  controls 
the  synthetic  camera  which  generates  views  of  the  3d  scene.  A  number  of  views  stored 
in  a  file  can  be  retrieved  any  time  and  new  views  generated  by  mouse  interaction  in 
the  graphics  window  can  be  stored.  The  use  of  the  Views  menu  is  explained  in  the 
User  Manual  (appendix  C,  page  142). 


4.3  Generation  of  a  Human  Figure  Model 


Before  one  can  start  to  animate  a  human  figure,  a  model  appropriate  to  the  given  goal 
needs  to  be  generated.  Since  this  work  is  not  concerned  with  soft  tissue  animation, 
a  stick  figure  model  is  sufficient  to  represent  its  movements.  The  model  of  the  actor 
consists  of  rigid  3d  body  parts  that  represent  the  limbs  of  the  body.  The  limbs  are 


36 


Swetlana  Gaffron 


a;  Views 


i  •  m 


View  Saver 


E  ec  or  d  j  Display  ;i  i 


t i / . 3ddemoviews  I 


Figure  4-5:  Views  menu  developed  by  Steven  Drucker 
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joined  together  forming  several  kinematic  chains.  Each  kinematic  chain  is  defined 
using  the  Denavit-Hartenberg  notation  explained  in  section  4.1.3. 


4.3.1  Generating  the  Body  Parts 

An  object  can  be  instanced  in  3d,  if  a  file  with  the  object  data  in  the  so  called  OSU 
format  exists  [11].  To  generate  data  files  in  OSU  format  for  rotational  symmetric 
objects,  a  tel  procedure  named  make-bone-data  has  been  written  (see  appendix  B, 
page  130).  The  parameters  the  procedure  needs  as  input  are  as  follows: 

make-bone-data  name  precision  nrrad  rl  11  r2  12  ...  rn 

Alternately,  the  radii  and  the  length  between  two  radii  have  to  be  specified.  To  in¬ 
fluence  the  shape  of  the  cross-section,  different  precisions  (number  of  points  forming 
the  cross-section)  can  be  specified,  e.g.  a  precision  of  3  would  correspond  to  a  tri¬ 
angular  cross-section,  whereas  4  would  yield  a  square  cross-section  and  so  forth.  As 
an  example,  a  data  file  rot .  asc  for  the  rotational  object  shown  in  figure  4-6  can  be 
generated  by  the  command: 

make-bone-data  rot  15  600234023200 


Figure  4-6:  Rotational  object  that  can  be  generated  using  the  make-bone-data  pro¬ 
cedure 

Most  of  the  body  parts  have  been  constructed  using  the  make-bone-data  procedure, 
some  of  them  with  subsequent  scaling  in  one  direction  (e.g.  the  upper  body).  Some  of 
the  conical  body  parts  are  extended  by  half  a  sphere  of  corresponding  radius  at  each 
end  that  will  overlap  with  the  reversed  sphere  from  the  adjacent  limb.  This  way,  the 
transition  between  two  limbs  appears  smooth  while  bending  for  example  an  elbow, 
knee,  or  finger  joint  without  a  gap  opening  in  the  middle  . 
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Appendix  B,  includes  all  tel  procedures  used  to  generate  these  kinds  of  objects.  The 
two  procedures,  left-circle  (page  130)  and  right-circle  (page  135),  serve  to 
calculate  the  radii  and  length  between  every  two  radii  needed  to  form  a  quarter  of  a 
circle.  The  make-bone  procedure  (page  130)  automatically  generates  a  conic  object 
with  half  a  sphere  at  each  of  its  ends. 


4.3.2  Building  a  Kinematic  Structure  of  the  Body 

Figure  4-7  on  page  39  demonstrates  the  design  of  the  kinematic  structure  shown  on 
top  of  a  human  skeleton  with  the  outline  of  the  human.  The  base  of  the  actor  is 
located  in  the  pelvis.  Attached  to  this  center  is  a  chain  for  each  leg  that  goes  along 
the  ankle  to  the  heel  (as  shown  in  figure  4-8  a)  ).  Starting  at  the  ankle,  there  is 
another  chain  consisting  of  two  links  ending  at  the  tip  of  the  toes. 

The  dhparameters  used  to  construct  the  two  kinematic  chains  for  the  left  leg  are 
shown  in  table  4.1. 


dhc 

joint  i 

name 

0, 

di 

Of 

Of 

comment 

1-leg 

0 

base:  pelvis 

90° 

0 

0 

90° 

1 

90° 

0 

0 

-90° 

dummy 

2 

-90° 

0 

3.24 

O 

O 

o 

3 

hip 

0° 

0 

0 

co 

o 

o 

4 

90° 

0 

0 

90° 

5 

0° 

0 

16.69 

0° 

6 

knee 

0° 

-0.3 

14.64 

0° 

7 

ankle 

0° 

0 

0 

CO 

o 

o 

8 

90° 

0 

0 

-90° 

9 

0° 

0 

0 

0° 

10 

O 

O 

O 

-3.25 

2.21 

0° 

ee:  heel  tip 

Lfoot 

0 

base:  ankle 

90° 

-3.25 

-3.66 

0° 

1 

0° 

0 

0 

-90° 

dummy 

2 

ball 

0° 

0 

-3.66 

0° 

ee:  toe  tip 

Table  4.1:  Denavit-Hartenberg  parameters  for  the  left  leg  and  foot 

Also  attached  to  the  base  is  a  kinematic  chain  for  the  spine  all  the  way  up  to  the 
head  (see  dhparameters  in  table  4.2,  page  41).  The  chains  for  the  arms  originate  in 
the  center  of  the  chest.  Figure  4-8  b)  shows  the  arm  chain  from  joint  6  on.  The  dhpa¬ 
rameters  for  the  left  arm  are  given  in  table  4.3  (page  41).  Each  finger  is  represented 
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Figure  4*8:  The  kinematic  chains  of  a)  a  leg  and  b)  an  arm 

by  a  separate  chain  starting  at  the  wrist.  The  dhparameters  for  the  left  index  are 
given  in  table  4.4  (page  42). 


4.4  Making  It  Move 

No  attempt  has  been  made  yet  to  validate  the  developed  motor  programs  against 
clinical,  experimental  data.  That  will  be  left  for  future  work.  The  movements  are  a 
visual  approximation  to  the  normal  human  velocity.  This  assumption  is  embodied  in 
two  global  variables  as  will  be  explained  in  the  following  section. 


4.4.1  Velocities 

Velocities  are  controlled  by  two  global  variables  depending  on  the  kind  of  a  skill. 
When  dealing  with  inverse  kinematics  skills,  the  end  effector  velocity  is  controlled 
by  the  global  variable  dist_per_timestep  that  specifies  the  average  distance  the  end 
effector  should  travel  per  timestep  (see  discussion  in  section  4.7.2).  For  all  other  skills, 
the  joint  velocities  are  controlled  by  the  global  variable  angle_per_timestep.  This 
variable  specifies  the  average  joint  angle  change  per  timestep.  In  case  of  a  skill  that 
brings  a  limb  into  a  target  configuration,  the  variable  angle_per_timestep  will  be 
applied  to  the  joint  that  has  to  overcome  the  maximum  angle.  The  angle  stepsizes  for 
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dhc 

joint  i 

name 

0, 

di 

di 

a, 

spine 

0 

base:  pelvis 

0° 

0 

0 

-90° 

1 

-90° 

0 

0 

90° 

2 

0° 

0 

7.1 

0° 

3 

waist 

-90° 

0 

0 

-90° 

4 

90° 

0 

0 

90° 

5 

90° 

0 

12.78 

-90° 

6 

neck 

-90° 

0 

0 

-90° 

7 

90° 

0 

0 

90° 

8 

90° 

0 

4.26 

-90° 

9 

head 

-90° 

0 

0 

-90° 

10 

90° 

0 

0 

90° 

11 

90° 

0 

0 

-90° 

Table  4.2:  Denavit-Hartenberg  parameters  for  the  spine 


dhc 

joint  i 

name 

0, 

di 

a  i 

a, 

local  motor  program 

l_arm 

0 

base:  chest 

0° 

0 

0 

CO 

o 

o 

move_shoulder 

1 

10° 

0 

0 

o 

O 

05 

lift/drop  shoulder 

2 

0° 

0 

5.3 

0° 

(not  used) 

3 

co 

o 

o 

0 

0 

CO 

o 

o 

(not  used) 

4 

100° 

0 

0 

o 

o 

05 

(not  used) 

5 

0° 

0 

2.3 

0° 

(not  used) 

6 

shoulder 

0° 

0 

0 

-90° 

lift.arm_sideward 

7 

o 

o 

CO 

0 

0 

co 

o 

o 

lift.armJorward 

8 

50° 

0 

0 

90° 

turn_arm 

9 

CO 

o 

o 

0 

10 

0° 

(dummy) 

10 

elbow 

o 

o 

o 

r*H 

0 

0 

90° 

bend.elbow 

11 

110° 

0 

0 

90° 

turn_hand 

12 

90° 

0 

8.6 

90° 

(dummy) 

13 

wrist 

0° 

0 

0 

90° 

bend_hand 

14 

0° 

0 

0 

0° 

twist_hand 

15 

0° 

-1.27 

2.5 

0° 

ee:  hand  palm 

Table  4.3:  Denavit-Hartenberg  parameters  for  the  left  arm 


42 


Swetlana  Gaffron 


dhc 

joint  i 

name  0,- 

di 

a,- 

a, 

angle  name 

index 

0 

base:  wrist  -12° 

0 

2.95 

0° 

(fixed) 

1 

12° 

0 

0 

-90° 

spreadJinger 

2 

10° 

0 

1.14 

0° 

bend_fingerl 

3 

20° 

0 

0.83 

0° 

bend_finger2 

4 

13.33° 

0 

0.64 

0° 

bend_finger3 

Table  4.4:  Denavit-Hartenberg  parameters  for  the  left  index 


dhc 

joint  i 

name  0, 

di 

a,- 

a,- 

angle  name 

thumb 

0 

base:  wrist  —30° 

0 

1.35 

0° 

(fixed) 

1 

-10° 

0 

0 

-90° 

spread-thumb 

2 

CO 

o 

o 

0 

1.12 

0° 

thumb-down 

3 

90° 

0 

0 

90° 

(dummy) 

4 

90° 

0 

0 

-90° 

thumb-turn 

5 

-60° 

0 

0.81 

0° 

bend_thumbl 

6 

15° 

0 

0.64 

0° 

bend_thumb2 

Table  4.5:  Denavit-Hartenberg  parameters  for  the  left  thumb 
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all  other  joints  will  be  synchronized  accordingly  so  that  all  joints  reach  their  target 
angle  at  the  same  timestep. 

4.4.2  Local  Motor  Programs 

The  main  focus  of  the  presented  work  was  put  on  skills  that  can  be  performed  with 
the  arm,  hand,  and/or  fingers,  i.e.  reaching  and  grasping.  Based  on  the  structure 
of  a  skeleton  arm  [8],  the  kinematic  chain  for  the  arm  provides  joints  for  the  clavicle 
and  the  scapula  (joints  0-5).  Only  two  of  them,  joint  0  and  joint  1,  are  used  to 
move  the  shoulder  for-  and  backward  as  well  as  up  and  down.  For  reasons  explained 
in  section  4.1.3,  there  are  two  dummy  joints  in  the  arm,  i.e.  joint  9  and  joint  12. 
The  remaining  7  joints  control  the  following  joint  motions:  arm  lifting  sideward,  arm 
lifting  for-  and  backward,  arm  turning  (about  the  upper  arm),  elbow  bending,  hand 
turning,  hand  bending,  and  hand  twisting.  Each  of  those  joint  motions  can  be  invoked 
by  a  corresponding  local  motor  program,  e.g.  bencLelbow  by  50  25  causes  the  elbow 
to  bend  by  50  degrees  in  25  timesteps  (see  proc  bend_elbow  on  page  113  in  appendix 
B). 

For  the  arm,  there  are  the  following  local  motor  programs  available: 

move_shoulder  option  angle  timesteps  renopt 

lift/drop-shoulder  option  angle  timesteps  renopt 

lift_arm_sideward  option  angle  timesteps  renopt 

lift_arm_f orward  option  angle  timesteps  renopt 

turn_arm  option  angle  timesteps  renopt 

bend.elbow  option  angle  timesteps  renopt 

turn_hand  option  angle  timesteps  renopt 

bend_hand  option  angle  timesteps  renopt 

twist_hand  option  angle  timesteps  renopt 

The  parameters  to  a  local  motor  program  are  the  option,  which  specifies  whether 
the  appropriate  joint  should  be  moved  “by”  the  specified  angle  or  “to”  the  specified 
joint  position,  the  joint  angle  (be  it  the  absolute  or  relative  one),  the  number  of 
timesteps  in  which  the  motion  should  be  performed,  and  the  renopt  that  can  be  set 
to  “ren”  or  “noren”  depending  on  whether  the  image  shall  be  updated  on  the  screen, 
i.e.  rendered,  with  every  timestep  or  not. 

Bending  and  turning  or  twisting  inwards  are  considered  to  change  a  joint  angle  in 
positive  direction.  A  fully  extended  or  untwisted  limb  corresponds  to  a  neutral  joint 
angle  of  0°. 
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For  not  every  joint  is  a  change  in  the  corresponding  dhtheta  value  (also  referred  to 
as  dhangle  0,  dhparameter  0,  or  simply  0)  consistent  with  the  above  convention. 
Sometimes,  the  direction  of  dhtheta  has  to  be  reversed  to  convert  into  the  real  joint 
angle,  denoted  as  $.  Table  4.6  shows  a  “-1”  in  the  “dir”  column  if  this  is  the  case. 
Also,  there  might  be  an  offset  between  0  and  $  (see  column  “off”  in  the  same  table). 
Finally,  the  conversion  from  $  to  0  can  be  calculated  by 

0  =  $  *  dir  -f  of  f  (4.3) 

and  is  noted  in  the  column  named  dhangle  0. 

Table  4.6  also  shows  the  joint  limits  for  the  joint  angles  $  and  their  corresponding 
limits  in  the  dhparameter  0  according  to  the  conversion  equation  4.3.  The  joint 
limits  are  recorded  in  the  procedures  get_max_angle  and  get_min_angle  included  in 
appendix  B,  page  123. 

To  access  a  current  joint  angle  4>,  there  is  a  procedure  get_/ozniname_angle  provided 
for  every  joint  that  reads  the  dhtheta  for  the  joint  in  question  and  converts  it  back 
to  the  real  joint  angle  4>.  See  procedure  get_elbow_angle  on  page  123  of  appendix 
B  for  an  example.  Every  local  motor  program  consists  of  three  main  parts: 

1.  Joint  limit  test: 

The  procedure 

in_range  limb  joint_nr  option  angle  min_angle  max_angle  restpos 
off  direc  part  action 

(see  appendix  B,  page  129)  tests  whether  the  desired  motion  exceeds  the  joint’s 
limits.  If  it  does,  the  procedure  returns  an  appropriate  reachable  angle  closest 
to  the  one  specified.  Otherwise  it  returns  the  angle  itself. 

2.  Angle  conversion: 

Conversion  from  the  desired  joint  angle  $  to  the  dhangle  0,  depending  on  how 
the  option  parameter  is  specified  (equation  4.3). 

3.  Joint  angle  change: 

The  change  of  the  corresponding  dhtheta  value  is  invoked  by  the  procedure 

move_a_joint  limb  joint_nr  option  dhangle  timesteps  range_angle  renopt 

(see  appendix  B,  page  131)  which  is  the  core  of  every  local  motor  program.  The 
option  parameter  can  be  set  to  “to”  or  “by”  which  provokes  either  a  movement 
to  a  specified  absolute  angle  or  by  a  specified  relative  angle. 
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dhangle 

name 

$  limits 

dir 

off 

dhangle  0 

0  limits 

0o 

shoulder_fb 

-20  -  40 

-1 

0 

-4* 

-20  -  40 

0i 

shoulderJd 

-5  -  45 

1 

0 

$ 

-5  -  45 

06 

armJs 

-180  -  180 

1 

0 

$ 

-180  -  180 

07 

arm-lf 

-90  -  190 

-1 

90 

-$  +  90 

180  -  -100 

08 

arm_turn 

-170  -  175 

1 

0 

$ 

-170  -  175 

010 

elbow 

0  -  150 

1 

90 

$  +  90 

90  -  240 

0ii 

hand-turn 

-40  -  190 

.1 

90 

$  +  90 

50  -  280 

013 

hand_bend 

-80  -  80 

1 

0 

$ 

-80  -  80 

014 

hand_twist 

-35  -  35 

-1 

0 

-$ 

35  -  -35 

Table  4.6:  Transformation  between  dhangle  and  real  joint  angles  and  joint  limits 


Local  motor  programs  to  control  the  finger  and  thumb  joints  are: 


spread-finger 
bend_a_f  inger_joint 
move_thumb_outwards 
turn-thumb 
move_thumb_down 
bend_thumb  _j  oint 


finger  option  angle  timesteps  renopt 

finger  joint  option  angle  timesteps  renopt 
option  angle  timesteps  renopt 
option  angle  timesteps  renopt 
option  angle  timesteps  renopt 
joint  option  angle  timesteps  renopt 


These  local  motor  programs  are  controlled  by  the  same  parameters  as  the  local  mo¬ 
tor  programs  for  the  arm  (explained  earlier  in  the  text).  The  spreadJfinger  and 
bend_a_f  inger.joint  procedures  have  an  additional  parameter  to  specify  the  finger 
in  question,  including  the  thumb. 

When  specifying  “thumb”  as  input  for  the  f  inger  parameter,  the  procedure  spread_- 
f  inger  calls  the  move_thumb.outwards  procedure  and  is  therefore  in  this  case  equiv¬ 
alent  to  the  latter  one.  In  the  same  way,  the  procedure  bend_a_f  inger_joint  with 
“thumb”  as  input  for  the  finger  parameter  calls  bend_thumb_joint,  see  the  source 
code  on  page  113  in  appendix  B. 

To  control  the  remaining  2  dofs  of  the  thumb,  the  procedures  move_thumb_down  and 
turn-thumb  are  provided.  Refer  to  tables  4.4  and  4.5  (page  42)  to  find  out  which 
dhtheta  correspond  to  which  local  motor  program. 

Should  all  finger  bending  angles  of  one  finger  be  bent  by  the  same  amount,  the 
procedure 


bend_one_f inger  finger  option  angle  timesteps  renopt 
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provided  for  convenience  can  be  called. 

As  Rijpkema  and  Girard  mention  in  [22],  it  is  almost  impossible  for  a  human  to  move 
the  last  link  (joint  4)  of  a  finger  without  moving  the  next  to  last  joint  (joint  3)  and 
vice-versa,  without  forcing  one  of  the  two  to  move  in  some  unnatural  way. 

Experimental  data  taken  by  Rijpkema  and  Girard  have  led  them  to  the  assumption 
that  the  dependency  of  those  last  two  joint  angles  could  be  reasonably  approximated 
by  a  linear  relationship: 

04  «  2/3  *  03  (4.4) 

The  bend_a_f  inger.joint  procedure  therefore  only  accepts  “1”  or  “2”  as  input  for 
its  parameter  joint  that  specifies  the  number  of  the  bending  joint  being  moved. 
According  to  table  4.4  the  first  two  bending  joints  of  a  finger  correspond  to  the 
dhparameter  02  and  03.  04,  the  dhparameter  corresponding  to  the  third  bending 
joint  will  automatically  be  changed  according  to  equation  4.4  with  a  change  in  03. 
The  last  two  links  of  the  thumb  are  coupled  in  the  same  way. 


4.4.3  Concurrent  Execution 

Performing  composite  skills  often  means  that  several  atomic  skills  have  to  be  executed 
concurrently  (see  section  4.7.3).  Also,  the  actor  might  have  different  tasks  at  the  same 
time,  e.g.  reach  for  a  glass  (which  is  a  composite  skill  itself)  and  move  his  head  and 
eyes  towards  the  glass.  A  command  loop  is  introduced  to  realize  the  concurrent 
execution  of  several  procedures. 

The  idea  of  the  command  loop  is  to  keep  a  list,  the  command  list,  of  all  commands 
that  should  execute  concurrently  and  to  loop  constantly  through  the  list.  Every 
procedure  that  should  execute  at  a  particular  time  has  an  entry  in  the  command  list 
at  that  time.  Procedures  on  the  command  list  can  be  control  procedures  for  specific 
motor  programs,  local  motor  programs,  or  procedures  that  provoke  a  change  in  the 
environment  of  the  virtual  actor,  such  as  procedures  to  move  an  object. 

The  procedure  controlling  the  command  loop  is  the  server  that  can  be  looked  at  on 
page  136  (in  appendix  B).  After  executing  each  of  the  commands  on  the  command 
list  once,  the  image  of  the  screen  is  updated  by  calling  the  rendering  routine.  Pro¬ 
cessing  several  different  local  motor  programs  by  small  steps  gives  the  impression  of 
concurrent  movement.  The  command  loop  can  be  started  and  run  uninterruptedly 
or  can  be  progressed  step  by  step. 

For  example,  bending  the  elbow  by  50  degrees  in  25  timesteps  is  realized  by  adding 
the  command  bend_elbow  2  1  to  the  command  list  and  removing  it  after  25  steps 
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through  the  command  loop. 

A  procedure  that  returns  either  “limit”,  “target”,  or  “stop”  removes  itself  from  the 
command  list.  Beyond  that,  motor  program  control  procedures  posses  the  ability 
to  add  or  remove  other  procedures  from  the  command  list.  The  server  takes  into 
account  that  new  procedures  might  have  been  added  to  the  list,  and  that  they  should 
be  executed  before  the  end  of  the  current  step  through  the  loop. 

Access  procedures  that  are  able  to  add,  insert,  or  remove  commands  from  the  com¬ 
mand  list  are  add_c,  insert.c,  and  rem_c  (included  in  appendix  B  pages  109,  129, 
and  135  respectively). 


Command  Loop 


!□ 


Command  Loop  ^stopptd) 
Co  j  One  Step  j 


Stop 


Command  List 


- - 


Update  List 


0  wave  hand  control 

1  direct  forearm  statel  {0.48  -0,59  0.653}  0  33 

2  align  hand  paint/point  axis  statel  {0 -10}  no  139 


Add  r 


:  Remove  # 


Delete  List 


Do  Command  Once 


Figure  4-9:  Command  Loop  menu 
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For  convenience,  an  X  menu  is  provided,  i.e.  the  Command  Loop  menu,  that  shows 
the  current  command  list  and  has  buttons  to  start,  stop  or  step  through  the  list  (see 
figure  4-9).  At  the  end  of  every  step  through  the  list,  all  new  x  events  are  flushed. 
This  way,  the  actor  can  react  to  any  command  typed  in  the  Do  Command  Once 
text  field  at  any  time.  Once  the  command  loop  is  started,  the  actor  can  only  be 
controlled  by  using  the  Command  Loop  menu. 

The  system  has  an  internal  clock  implemented  as  a  global  variable  that  is  increased 
by  one  timestep  every  step  through  the  command  loop. 


4.5  Inverse  Kinematics  Algorithms 

The  basic  inverse  kinematics  algorithm  for  the  arm,  explained  in  this  chapter,  is  used 
to  position  either  the  wrist  or  the  end  effector  of  the  hand  at  a  specific  point  in  space. 
The  end  effector  is  generally  located  in  the  center  of  the  palm.  This  algorithm  will 
be  later  extended  in  order  to  obtain  appropriate  joint  angles  for  all  joints  involved  to 
synchronously  reach  a  target  position  and  orientation  of  the  end  effector. 

Four  joint  angles  are  taken  into  account  by  the  basic  inverse  kinematics  algorithm: 
the  two  angles  of  the  spherical  joint  at  the  shoulder  that  determine  the  upper  arm 
orientation,  (i.e.  the  arm-lifting-forward  angle  (or  arm-lf  angle )  and  the  arm-lifting- 
sideward  angle  (or  arm-ls  angle)),  the  elbow-bending  angle  (or  elbow  angle),  and  the 
arm-turn  angle. 

If  the  wrist  is  to  be  positioned,  the  hand  angles  (turning,  bending,  and  twisting)  do 
not  play  a  role.  For  the  positioning  of  the  end  effector  of  the  hand,  constant  hand 
angles  are  assumed  for  now. 

Having  to  determine  4  joint  angles  out  of  a  3  dimensional  coordinate  point  in  space 
leaves  1  degree  of  freedom.  This  is  used  to  influence  the  x-otation  of  the  arm  about  the 
vector  from  the  shoulder  to  the  goal  point.  A  parameter,  called  arm_pose_parameter 
is  introduced  in  order  to  specify  the  additional  degree  of  freedom.  The  parameter  can 
take  any  value  between  0  and  1,  where  0  will  result  in  a  position  with  the  elbow  at  the 
lowest  possible  point  and  1  in  an  elbow  position  at  the  highest  possible  point.  The 
elbow  is  always  assumed  to  be  on  the  outside  of  the  plane  defined  by  the  upper  arm, 
and  the  forearm  in  the  lowest  elbow  position,  as  viewed  from  the  center  of  the  body. 
As  a  global  variable,  the  arm_pose_parameter  can  be  changed  any  time  during  the 
execution  of  inverse  kinematics  routines  by  the  procedure  app  (included  in  appendix 
B  on  page  112). 

The  basic  inverse  kinematics  algorithm  is  implemented  in  a  procedure  called  goal- 
_arm_angles_abs  (see  appendix  B,  page  124)  that  takes  two  parameters  as  input:  the 


4.5  Inverse  Kinematics  Algorithms 


49 


goal_point,  a  three  dimensional  coordinate  point  in  world  space  coordinates,  and 
gp_opt,  the  goal  point  option  that  can  be  either  set  to  “wrist”  or  “ee”  to  specify 
whether  the  wrist  or  the  end  effector  is  to  be  positioned  at  the  given  goal  point.  The 
procedure  returns  the  absolute  angles,  i.e.  the  arm-ls  angle,  the  arm-lf  angle,  the  arm- 
turn  angle,  and  the  elbow  angle,  necessary  to  reach  the  goal  point.  The  algorithm 
is  divided  into  two  major  parts  that  will  be  explained  in  more  detail  in  section  4.5.1 
and  4.5.2.  These  are: 

1.  Calculation  of  the  shoulder-elbow- vector  and,  in  case  gp.opt  is  set  to  “wrist”, 
calculation  of  the  elbow-wrist-vector. 

2.  Inverse  kinematics  calculation  to  determine  the  joint  angles. 

Adaptation  to  a  moving  body,  shoulder  movements  and/or  changes  in  the  hand  angles 
will  take  place  by  recomputing  the  inverse  kinematics  algorithm  every  timestep. 

Often  it  is  desired  not  only  to  place  the  end  effector  or  wrist  at  a  specific  point  in 
space  but  also  to  reach  a  specific  orientation  of  the  hand.  Imagine  washing  windows. 
The  end  effector  should  follow  a  looplike  trajectory  in  the  plane  of  the  window,  while 
the  hand  plane  should  stay  parallel  to  the  plane  of  the  window  all  the  time.  Another 
example  is  grasping  an  object.  If  the  actor  grasps  a  glass,  it  should  bring  its  end 
effector  together  with  a  specific  point  on  the  glass  and  simultaneously  align  its  thumb 
axis  (see  figure  4-11  on  page  57)  with  the  longitudinal  axis  of  the  glass. 

In  section  4.5.3,  the  inverse  kinematics  calculations  for  aligning  the  hand  axes  will  be 
described.  Section  4.5.4  then  shows  how  to  integrate  reaching  for  a  goal  point  and 
the  alignment  of  the  hand. 


4.5.1  Calculation  of  the  shoulder-elbow-vector  and  the  elbow- 
wrist-vector 

Figure  4-10  shows  the  arm  positioned  such  that  the  end  effector  (ee)  coincides  with 
a  given  goal  point. 


Calculation  of  the  shoulder-elbow-vector 

The  vector  from  the  shoulder  to  the  elbow  is  calculated  by  the  procedure  calc_shoul- 
der_elbow_vector  (see  appendix  B,  page  116).  From  all  possible  solutions  for  the 
shoulder-elbow-vector  (sev)  one  is  chosen  by  taking  the  arm_pose_parameter  into 
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Figure  4-10:  Vectors  defining  the  arm  links 


account.  In  order  to  calculate  sev,  the  angle  7  enclosed  by  sev  and  the  shoulder-goal- 
vector  ( sgv )  has  to  be  computed  first.  7  can  be  computed  out  of  the  lengths  of  the 
vectors  sgv,  sev,  and  eeev  (the  vector  from  the  elbow  to  the  end  effector): 


7  =  arccos 


|seu|2  -f  |spw|2  —  |eeeu|2> 
2  |seuj  |s<7v| 


(4.5) 


Equation  (4.5)  is  implemented  in  procedure  gamma  included  in  appendix  B,  page  123. 

|s<fu|,  i.e.  the  distance  from  the  shoulder  to  the  goal  point,  is  known  since  the  shoulder 
is  assumed  to  remain  at  its  current  position,  |sev[  is  the  length  of  the  upper  arm,  and 
|eeei;|  is  the  distance  between  the  current  elbow  position  and  the  current  end  effector 
position  that  will  not  change  if  the  hand  angles  stay  the  same. 

3d  provides  a  command  that  returns  the  end  effector  position  (dhgetee),  whereas  the 
current  positions  of  all  joints  can  be  accessed  through  the  corresponding  dhmatrices 
(see  e.g.  procedure  get_shoulder_position,  appendix  B,  page  124). 

In  case  not  the  end  effector,  but  the  wrist,  is  to  be  positioned  at  the  goal  point,  \eeev\ 
has  to  be  substituted  by  the  length  of  the  elbow-wrist-vector  (\ewv\),  i.e.  the  forearm 
length. 

All  possible  solutions  for  the  shoulder-elbow-vector  are  located  on  a  circle  about 
|sgt;|.  Supposed  z3ev,  the  z  coordinate  of  jseuj  is  known,  (it  will  later  be  shown  how 
to  determine  zsev ),  the  other  two  coordinates  can  be  computed  using  the  following 
two  equations: 

\sev\2  =  x]ev  +  y]ev  +  z]ev 


«S6U|  COS  'y  %sev  S'sgv  4"  Vsev  t/sgv  4"  %sev  %sgv 


(4.6) 

(4.7) 
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where  equation  (4.7)  is  nothing  else  but  the  definition  of  the  dot  product  of  two 
vectors. 

Solving  equations  (4.6)  and  (4.7)  yields  two  solutions  for  xaev  : 


(4.8) 


with 


P  = 


2  X3gV  ( Z 


sev  %sgv 


|seu|  cos  7) 


x 


sgv 


+  y. 


2 

sgv 


(4.9) 


sew|  cos 7  (|sei>|  \sgv\  cos  7  —  2  zsev  zsgv)  +  z2sev  {y]gv  +  z]gv )  —  |seu|2  y]gv 


x 


sgv 


+  y2sgv 


(4.10) 


Equation  (4.8)  is  implemented  in  procedure  calc_x2  included  in  appendix  B,  page  118. 
For  the  left  arm,  the  solution  with  the  greater  xaev  will  always  be  chosen  so  to  keep  the 
elbow  turned  “outside”,  which  is  the  more  natural  of  the  two  solutions.  ysev  will  then 
be  calculated  using  equation  (4.6)  (see  procedure  calc_third_component,  appendix 
B,  page  118).  Whether  the  positive  or  negative  yaev  is  the  valid  solution  will  be 
verified  by  means  of  procedure  vangle2  (see  appendix  B,  page  138)  that  calculates 
the  angle  between  the  resulting  sev  and  sgu.The  resulting  angle  will  be  compared 
with  the  angle  7  computed  by  equation  (4.5). 

A  valid  range  for  zaev  can  be  computed  from  the  condition  that  the  root  argument  in 
equation  (4.8)  has  to  be  greater  than  or  equal  to  zero  (see  procedure  calc_z2_range  in 
appendix  A,  page  118).  The  zsev  range  will  be  mapped  onto  the  arm_pose_parameter 
(app)  mentioned  earlier  on  page  112,  where  an  app=0  refers  to  zse Vmin  and  an  app=l 
refers  to  zaeVm„. 

The  calc_shoulder_elbow_vector  procedure  also  checks  whether  the  computed  so¬ 
lution  would  end  up  in  an  elbow  position  inside  or  too  close  to  the  body,  in  which 
case  it  would  recompute  the  algorithm  to  find  a  better  solution.  The  upper  arm 
orientation  results  directly  from  the  shoulder-elbow- vector. 


Calculation  of  the  elbow-wrist-vector 

If  it  is  the  wrist  that  is  to  be  positioned  at  the  goal  point,  the  elbow-wrist-vector 
can  be  obtained  easily.  Subtracting  the  shoulder-goal-vector  from  the  just  calculated 
shoulder-elbow-vector  yields  the  vector  from  the  elbow  to  the  wrist  (ewv)  : 


euw  =  sev  —  sgv 


(4.11) 


52 


Swetlana  Gaffron 


The  elbow-wrist-vector  determines  the  forearm  orientation,  i.e.  the  target  axis  of  the 
forearm. 


4.5.2  Inverse  Kinematics  Algorithms  for  Orienting  the  Arm 
Links 


Inverse  kinematics  has  to  be  applied  in  order  to  calculate  the  arm-ls  and  arm-lf  angle., 
that  result  in  the  desired  orientation  of  the  upper  arm  (procedure  upper_arm_an- 
gles_abs).  In  case  the  gp_opt  is  set  to  “wrist”  (refer  to  page  49),  inverse  kinematics 
must  again  be  applied  to  calculate  the  elbow  and  arm-turn  angle,  given  the  orientation 
of  the  forearm  (procedure  forearm_angles_abs). 

In  case  of  a  gp_opt  “ee”,  another  inverse  kinematics  algorithm  is  used  to  calculate  the 
elbow  and  arm-turn  angle,  given  the  resulting  coordinate  frame  at  the  elbow  joint, 
i.e.  the  dhmatrix  M7.  M7  depends  on  the  new  arm-ls  and  arm-lf  angles  as  well  as 
on  the  goal  point  itself  (procedure  calc_at_e). 

Knowing  the  dhmatrix  Mm  of  a  joint  m  that  is  temporarily  fixed  in  space  and  knowing 
parts  of  the  target  dhmatrix  Mn  for  a  joint  n  located  farther  along  the  same  dhchain, 
equation  (4.2)  (page  29)  makes  it  possible  to  calculate  joint  angles  for  all  joints  in 
between  m  and  n.  After  premultiplying  equation  (4.2)  with  the  inverse  of  Mm 

Am+l  '  Am+2  • ...  •  An-l  •  An  =  Mm  •  Mn  5  {n  >  wi)  (4-12) 

a  set  of  equations  can  be  formulated  containing  trigonometric  functions  of  the  joint 
angles  in  question.  By  skillful  transformation  of  the  resulting  equation  system,  it  is 
possible  to  eliminate  the  joint  angles  one  by  one. 


Aligning  the  upper  arm  along  a  given  axis:  computing  the  upper  arm 
angles 

The  upper  arm  angles  arm-ls  angle  and  arm-lf  angle  correspond  to  the  dhangles  06 
and  67,  which  can  be  calculated  according  to  matrix  equation  (4.12): 

Ac  •  A7  =  Mj1  •  M7 


(4.13) 
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Taking  the  dhparameters  for  joints  6  and  7  from  table  4.3  and  putting  them  into  the 
transformation  matrix  (4.1)  (on  page  29),  yields 


f  Ce 

0 

-56 

0 

\ 

Ag  = 

^6 

0 

Ce 

0 

0 

-1 

0 

0 

l  0 

0 

0 

1 

) 

where  e.g.  denotes  cos  06  etc. 


f  C7  0  Sj  0  > 

Sj  0  — C7  0 

0  10  0 
Vo  0  0  1  / 


(4.14) 


The  target  axis  of  the  upper  arm  that  is  given  by  the  shoulder-elbow-vector  is  the 
third  vector  b  defining  the  coordinate  frame  at  joint  7  and  is  therefore  contained  in 
dhmatrix  M7  as  follows: 


M7 


(  *  *  bQ  *  \ 

*  *  b\  * 

*  *  62  * 

V  0  0  0  1  / 


(4.15) 


Multiplying  A6  and  A7  from  equations  (4.14)  and  defining 

M51  -b=B 

the  third  column  of  equation  (4.13)  can  be  written  as 


1  C6  57  ^ 

( B°  \ 

56  57 

B1 

C7 

b2 

V  0  ) 

\  0  / 

(4.16) 


(4.17) 


In  tel ,  the  calculation  of  vector  B  is  implemented  as  follows 


Mset  [dhmatrix  5  /l_arm/dhc] 

Minvert 

Male  300 

Mele  310 

Mele  320 

set  B  [Mxform  $main_axis] 


Solving  equation  (4.17)  for  06  and  07,  results  in 
07a  =  arccos  B2 


07i,  =  —  arccos  B2 


(4.  IS) 
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06a  =  arccos  (-r^r-)  ©6b  =  - arccos  (-r^r)  (4-!9) 

Depending  on  the  joint  limits  (see  table  4.6,  page  45),  valid  solutions  for  07  and  then 
for  06  will  be  chosen.  In  case  both  solutions  for  06  or  07  are  valid,  the  one  closer 
to  the  current  joint  position  will  be  chosen.  Sometimes  both  solutions  are  outside 
of  the  allowed  joint  angle  range,  in  which  case  0  will  be  set  to  its  closest  limit  and 
a  warning  message  will  be  printed  out  saying  that  the  target  upper  arm  axis  is  not 
reachable.  06  has  to  be  verified  using  the  second  equation  resulting  from  (4.17). 

Finally,  06  and  07  are  converted  into  the  arm-ls  and  arm-lf  angle,  using  equa¬ 
tion  (4.3).  The  described  computation  is  implemented  in  procedure  upper_arm_- 
angles_abs  (see  appendix  B,  page  137). 


Aligning  the  forearm  along  a  given  axis:  computing  the  forearm  angles 

The  following  method,  implemented  in  procedure  f  orearm_angles_abs  (appendix  B, 
page  121),  is  applied  in  case  the  goal  point  is  to  be  positioned  at  the  wrist,  (i.e. 
gp_opt  is  set  to  “wrist”).  The  computation  of  the  forearm  angles  that  correspond 
to  dhangles  0s  and  0iO  is  very  similar  to  the  procedure  described  previously.  The 
axis  vector  of  the  forearm,  which  is  given  by  the  elbow-wrist-vector,  is  contained  in 
dhmatrix  Mio  in  the  third  column.  Taking  dhmatrix  M7  as  input  for  the  calculation, 
the  application  of  equation  (4.12)  yields 

As  •  Ag  •  A10  =  M7x  •  Mio  (4.20) 

which  results  in 
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(4.21) 
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after  multiplying  the  transformation  matrices  As  ■  A9  •  A10,  where  all  known  dhpa- 
rameters  are  put  in  according  to  table  4.3. 

With  the  definition 

Mf1  •  d  =  3  (4.22) 

equating  the  third  column  of  matrix  equation  (4.21)  yields 

0io6  =  180°  —  arcsin  D2  (4.23) 


0ioa  =  arcsin  D2 
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©8a  =  arccos  ( — —pr~)  086  =  - arccos  (4.24) 

Again,  a  solution  for  ©io  is  chosen  depending  on  the  limits  for  joint  10.  In  case 
of  further  ambiguity,  the  solution  that  is  closer  to  the  current  joint  angle  is  chosen. 
The  solution  for  08  is  verified  using  the  second  equation  resulting  from  (4.21).  After 
conversion  of  08  and  ©io,  the  procedure  forearm_angles_abs  returns  the  arm-turn 
angle  and  elbow  angle. 

Parameter  M7  is  assigned  to  the  procedure  f  orearm^angles.abs,  which  can  either 
be  set  to  the  current  dhmatrix  M7,  in  case  the  upper  arm  does  not  move,  or  to 
M7  calculated  using  new  angles  arm-ls  and  arm-lf  as  input  (see  procedure  calc_M7, 
appendix  B,  page  115).  For  the  inverse  kinematics  positioning  of  the  wrist  at  a  goal 
point,  M7  is  calculated  using  the  arm-ls  and  arm-lf  angles  (returned  by  the  procedure 
upper_arra_angles_abs). 


Computing  the  forearm  angles  based  on  a  given  goal  point  for  the  end 
effector 


This  computation  is  similar  to  the  previous  ones.  Since  the  hand  angles  are  assumed 
to  be  constant  and  this  time  the  goal  point  (<7o,  <71,02)  that  can  be  found  in  the  fourth 
column  of  dhmatrix  M15  is  known,  the  problem  can  be  reduced  to  the  solution  of 

A8  •  Ag  •  A10  •  An  *  A12  •  A13  •  A14  •  A15  =  •  M15  (4.25) 

After  defining  Atemp  =  An  •  A12  •  A13  ■  A14  ■  A15,  equation  (4.25)  can  be  written 
as 


(  — SioC8  s8  C10c8 

— — C8  Cio38 
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\  0  0  0 
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(4.26) 


where  Atemp  is  known  since  it  only  depends  on  the  hand  angles.  The  elements  of  the 
matrix  Atemp  will  be  assigned  to  variables  as  follows 


At 


cmp 


(  an  a12  a13  a14  ^ 

«21  a22  a23  «24 

«31  «32  033  O34 


\  0  0  0  \  ) 


(4.27) 
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With  the  definition 

Mf1  -g  =  G 

the  fourth  column  of  matrix  equation  (4.26)  looks  as  follows: 


(  — fll4c8510  +  O24S8  T  a34c8c10  ^ 

( G°  \ 

fll458^10  —  O24C8  +  a3458ClO 

Gx 

a14c10  —  a34510  +  10 

G2 

V  1  ) 

\  1  / 

(4.28) 


(4.29) 


and  yields 


^10a,i>  — 


—023  (10  —  G2) 


Ol4  +  a 


± 


34 


1 


«14  (034  +  a14  (10  —  f?2 )2  +  a?4) 


(o?4  +  4d2 


(4.30) 


f>8a,b  — 


2Gq«24 


a24  +  (O14S10  —  O340xo)2  \j 


(«2 4  ~  Gp)  (ai4.Sio  —  a34Cxo)2  -f  (ai4^10  ~  O34clo)4 
(«24  +  (O14S10  ~  O34C10)2)2 

(4.31.) 

In  each  case  (solution  a  as  well  as  solution  b  for  each  sin  0)  there  will  be  two  solutions 
for  0,  resulting  from  the  arcsin  function,  yielding  four  solutions  for  each  0.  Only 
two  of  the  four  solutions  in  each  case  are  true  when  verifying  them  with  one  of  the 
equations  from  4.29.  Of  the  two  remaining  solutions,  one  is  picked  that  is  within 
the  joints  limits  or,  if  both  are  within  the  limits,  the  one  closer  to  the  current  joint 
position  is  picked. 

As  in  procedure  f  orearm_angles_abs,  08  and  01O  will  be  converted  into  the  arm-turn 
angle  and  the  elbow  angle,  which  are  the  two  return  values  of  procedure  calc_at_e. 


4.5.3  Inverse  Kinematics  Algorithms  for  Orienting  the  Hand 
Axes 

Figure  4-11  shows  the  definition  of  the  hand  axes  on  the  left  hand.  There  are  two 
different  inverse  kinematics  algorithms  to  calculate  the  necessary  hand  angles:  one  for 
alignment  of  the  palm  axis  or  palm  and  point  axes  with  target  vectors  and  another 
one  for  alignment  of  the  thumb  axis  with  the  longitudinal  axis  of  an  object. 
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Figure  4-11:  Definition  of  the  hand  axes  shown  on  the  left  hand  (inner  palm) 
medium:  point  axis,  light:  thumb  axis,  dark:  palm  axis 


Computing  the  hand  angles  based  on  a  target  thumb  axis 


One  way  of  grasping  an  object  like  a  glass  is  to  align  the  thumb  axis  with  the  longitu 
dinal  axis  of  the  object,  and  then  close  the  fingers  around  the  object  once  approached. 
This  kind  of  grasping  is  the  only  kind  considered  for  this  thesis. 

The  computation  of  the  hand  angles,  given  a  thumb  axis,  is  based  on  equation  (4.12) 
so  that  the  problem  can  be  formulated  as  follows: 

An  •  A12  ■  A13  •  A14  =  MXq  •  M14  (4.32) 


The  negative  thumb  axis  is  contained  in  dhmatrix  M14  in  the  second  column 
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so  that  after  further  substitutions  (4.32)  yields 
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(4.33) 


(4.34) 
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with  the  solutions 


©14a 


t2 

=  arcsin - — — 

—  COS  013 


0!46  =  180°  -  014a  (4.35) 


0 


Ha 


arcsm 


T\  cos  0i4  4*  TqT2  tan  0i3 

,  W+ri  , 


01,1  =  ISO”  -  0 


11a 


(4.36) 


By  checking  the  joint  limits  and  considering  the  solutions  closer  to  the  current  configu¬ 
ration,  0i4  and  0ii  are  selected  from  (4.35)  and  (4.36).  ©13  can  be  freely  chosen  and  is 
therefore  provided  as  an  input  parameter  to  the  procedure  aligned_hand_angles_abs 
(see  appendix  B,  page  109),  in  form  of  the  hand-bend  angle.  Another  parameter  to 
that  procedure  is  M10  that  can  either  be  set  to  the  dhmatrix  M10  or  to  a  matrix  cal¬ 
culated  depending  on  arm-ls,  arm-lf,  arm-turn  and  elbow  angle  (procedure  calc_M10, 
appendix  B,  page  116). 


Computing  the  hand  angles  based  on  target  palm  and  point  axes 


In  the  same  way  as  explained  in  the  previous  sections,  the  hand  angles  for  given  palm 
and  possibly  also  pointing  axes  are  computed. 


Dhmatrix  M14  contains  the  palm  axis,  named  p,  and  the  pointing  axis,  named  n  as 
follows: 


m14 

The  right  side  of  equation  (4.32) 
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(4.37) 


(4.38) 


which  yields 
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(4.39) 
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and 


(  —  Sji  Ci3 

(  N°  \ 

Cll  Ci3 
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■Sl3 
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\  o  j 

\  0  } 

(4.40) 


From  (4.40)  it  becomes  obvious  that  the  hand-twist  angle  (corresponding  to  014) 
does  not  influence  the  palm  axis.  Hence,  it  is  possible  to  specify  only  a  palm  axis  as 
input  for  the  hand  angles  calculation  procedure  hand_angles_rel  (see  appendix  B, 
page  125).  In  this  case  the  procedure  returns  the  new  hand-turn  angle  and  the  new 
hand-bend  angle,  but  the  current  hand-twist  angle.  0i3  and  0n  will  be 


©i3a  =  arcsinA^  0136  =  180°  —  arcsin  (4-41) 

0iia  =  arctan  ©lit  =  180°  +  arctan  (4.42) 

Is  a  point  axis  for  the  hand  also  given,  then  0i4  can  be  computed  considering  the 

third  row  in  (4.39) 


0i4a  =  arccos 


0i4i>  =  —  arccos 


( 


-A_) 

COS  013/ 


(4.43) 


4.5.4  Inverse  Kinematics  Algorithm  for  Integrated  Posi¬ 
tioning  and  Orientation  of  the  Hand  Thumb  Axis 

For  synchronous  alignment  of  the  hand  thumb  axis  with  a  target  axis  while  reaching  a 
goal  point,  it  is  necessary  to  compute  the  final  hand  angles  depending  on  the  dhmatrix 
Mio  that  results  when  the  end  effector  is  positioned  at  the  goal  point.  Procedure 
reach_angles_abs  (see  appendix  A,  page  135)  combines  the  goal_arm_angles_abs 
procedure  that  calculates  the  arm-ls,  arm-lf,  arm-turn,  and  elbow  angle  with  the 
align_hand_angles_abs  procedure  (in  which  the  3  hand  angles  are  computed).  The 
matrix  Mio  is  computed  from  the  four  goal-arm-angles  by  the  procedure  calc_M10, 
mentioned  earlier,  and  used  as  input  for  the  computation  of  the  hand  angles. 


4.6  Collision  Avoidance 

Collision  avoidance  will  be  of  great  importance  once  the  actor  moves  in  a  more  com¬ 
plicated  environment,  performing  ever  more  complex  tasks.  The  collision  avoidance 
feature  implemented  as  part  of  this  thesis  is  limited  to  the  generation  of  a  collision 
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free  path  for  the  end  efFector  when  performing  a  reaching  motion  towards  an  object 
on  the  table. 

According  to  the  average  distance,  the  end  efFector  is  expected  to  travel  per  timestep 
(dist_per_timestep),  equidistant  goal  points  along  the  path  are  generated.  The  end 
efFector  is  supposed  to  move  sequentially  through  all  these  goal  points  in  order  to 
avoid  collision. 

The  procedure  that  calculates  points  on  a  path  around  a  collision  object  (collobj 
parameter),  i.e.  the  table  in  this  example  ,  is 

f ind_path_pos  object  collobj  showpath 

The  showpath  parameter  can  be  set  to  “yes”  which  will  cause  green  balls  to  be 
displayed  at  the  calculated  path  points  for  demonstration  purposes. 


Figure  4-12:  Division  of  the  space  to  find  a  collision  free  path  (vertical  cross-section) 

The  space  around  the  collision  object  viewed  in  the  yz-plane  is  divided  into  several 
areas  according  to  figure  4-12.  The  points  Pi  to  P5  will  be  calculated  depending  on 
the  position  of  the  collision  object  and  the  object  that  is  to  be  reached: 

• l  (  XPi  5  Umax  collobj  T  Vdisil  i  zmi In  collobj  —  zdist\  ) 

"^2  (  2,’p2  ,  ymax  collobj  T  Udistl  5  Z max  collobj  T  zdistl  ) 

^3  (  XP3  1  Umax  collobj  T  Vdisti  1  zmax  collobj  zdist2  ) 
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P4  —  (  *^P4  1  Umax  collobj  ,  ^mai  collobj  T  Zdist2  ) 

Ps  =  (  Tp5  j  Dobj  T  ydistZ  ,  Z0bj  *1"  ZdistZ  ) 

The  following  values  have  been  chosen  for: 


Udistl  —  5.2 

Vdist2  =  3 

ydistZ  —  1 

Zdistl  =  1 

%dist2  —  3.5 

ZdistZ  —  0*4 


(accounting  for  the  distance  between  the  end  effector  and  the  tip  of  the 
middle  finger  plus  a  safety  distance  of  2,  measured  in  inches). 


Depending  on  the  current  position  of  the  end  effector,  the  first  point  for  the  path 
will  be  chosen.  If  the  end  effector  is  located  in  area  3  for  example,  the  first  point 
on  the  path  will  be  P3  followed  by  P4,  P5,  and  finally  the  object’s  centroid  P0bj ,  see 
figure  4-13. 


Figure  4-13:  Collision  free  path  starting  from  an  end  effector  position  in  area  3 

The  x  values  of  the  path  points  depend  on  the  x  value  of  the  current  end  effector 
position  (.Tee),  and  the  position  of  the  object  on  the  table  ( x0\,j ).  If  the  body  is  very 
close  to  the  table,  and  the  end  effector  of  the  left  hand  is  on  the  right  side  of  the  left 
shoulder  joint,  then  the  path  should  lead  the  end  effector  to  move  just  slightly  to  the 
left  of  the  shoulder  joint  at  P\. 
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if  {($x_ee  <  $x_sp)  &  ($y_pl  >  -5)  &  ($x_obj  <  $x_ee)}  { 
set  x_pl  [plus  $x_sp  0.1] 

}  else  { 

set  x_pl  $x_eep 

> 


In  order  to  approach  the  object  with  the  left  hand  from  the  left  side,  xps  will  be  set 
to 


—  X obj  “1”  3 

and  the  other  x  values 


XP2  ~  XPl  » 


x 


P3 


—  xPi  + 


_  xP1  +xPS 
XP4  —  2 


In  case  yPs  is  greater  then  yPi ,  P4  will  be  skipped  completely. 

If  the  end  effector  is  located  in  area  5,  a  new  point  Pnew  will  be  inserted  between  the 
end  effector  and  P5  in  case  xee  is  less  than  x0bj,  see  figure  4-14.  The  coordinates  of 
Pnew  will  be 

P new  —  (  % obj  )  Vobj  T  ydistl  1  Zobj  T  ZdistS  ) 

r  *■  y 


Figure  4-14:  Insertion  of  Pnew  in  case  the  end  effector  is  located  in  area  5 

In  order  to  force  the  end  effector  to  move  along  the  collision  free  path  smoothly, 
equidistant  goal  points  have  to  be  generated  on  the  polygon  that  is  defined  by  all 
path  points  included  in  the  path.  Procedure 

make_equidistant_points  point-list 
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implements  the  following  algorithm: 

1.  Calculate  the  length  1  of  the  polygon  through  a  point  list  (p0  to  pn). 

2.  Parameterize  the  polygon,  store  the  parameter  t  at  each  point  of  the  point  list 
(PO  •  ^0?  •  •  ■  >  Pn  :  in  —  1)- 

3.  Find  the  number  of  goal  points  by  dividing  the  polygon  length  by  the  global  vari¬ 
able  dist_per_t imestep  and  finding  the  closest  integer  (nrgp  =  int  {1/ dist.- 
per-timestep )  ). 

4.  Adapt  the  stepsize  s  accordingly  (s  =  l/nrgp  ). 

5.  Set  the  first  goal  point  to  be  the  first  point  from  the  point  list  ( gp0  =  Po)- 

6.  Increase  the  parameter  along  the  polygon  by  the  stepsize  (  tgp-  =  +  s  ) 

and  check  between  which  points  (from  the  point  list)  the  new  goal  point  gpj  is 
located  (  ti  <  tgPj  <  t;+i  )• 

7.  Calculate  the  coordinates  of  the  new  goal  point  from  the  parameter  tgP}  and  the 
linear  equation  formed  by  p;  and  p;+i . 

8.  Repeat  steps  6  and  7  until  the  end  of  the  point  list. 

9.  Return  a  list  with  all  goal  points  calculated. 

Figure  4-15  shows  the  points  of  the  original  point  list  as  black  circles  labeled  P\  to  P$ 
and  the  original  polygon  as  a  thin  line.  The  new  equidistant  goal  points  are  marked 
by  crosses  and  the  new  polygon  is  drawn  bold. 

Finally,  the  procedure  equidistant_path_points  combines  both  previously  explained 
procedures  by  providing  the  point-list  returned  by  f ind_path_pos  as  input  for 
make_equidist  ant -points. 

The  implemented  collision  avoidance  algorithm  just  described  is  specific  for  the  left 
arm  and  guarantees  that  the  hand  will  move  from  its  current  position  to  the  object 
without  colliding  with  a  specified  collision  object,  i.e.  the  table.  However,  the  algo¬ 
rithm  does  not  yet  guarantee  collision  free  moves  of  the  other  arm  links,  i.e.  the  upper 
arm  and  the  forearm.  In  order  to  avoid  collisions  of  these  arm  links  it  is  necessary 
to  specify  an  appropriate  arra_pose_parameter.  Remember,  changing  this  parameter 
will  influence  the  position  of  the  elbow,  but  not  the  end  effector  position. 
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Figure  4-15:  Generation  of  equidistant  points  on  the  collision  free  path 

4.7  General  Structure  of  Motor  Programs 

Motor  programs  are  the  source  code  for  an  actor’s  skills  and  implemented  as  finite 
state  machines  as  explained  in  section  3.4.2. 

4.7.1  Motor  Programs  for  Atomic  Forward  Kinematics  Skills 

A  motor  program  for  an  atomic  skill  can  consist  of  several  tel  procedures,  but  has  at 
least  two:  the  main  motor  program  procedure  and  at  least  one  state. 

The  name  of  the  main  motor  program  procedure  is  the  skill’s  name.  Therefore  the 
execution  of  a  skill  is  invoked  by  typing  the  skill’s  name  in  the  Do  Command 
Once  text  field  of  the  Command  Loop  menu  (see  figure  4-9)  if  the  command  loop 
is  already  running.  Otherwise,  the  skill’s  name  can  be  typed  at  the  3d  >  prompt 
and  the  command  loop  can  be  started  thereafter  by  clicking  the  Go  button  in  the 
Command  Loop  menu. 

The  task  of  the  main  motor  program  procedure  is  to  check  whether  the  initial  con¬ 
ditions  for  a  skill  are  fulfilled  and  to  add  the  procedure  for  the  first  state  to  the 
command  list.  In  case  the  initial  conditions  are  not  fulfilled,  the  procedure  will  quit 
and  print  out  a  warning  message.  The  general  structure  of  such  a  main  motor  program 
procedure  looks  like  the  following: 
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proc  skilLname  {  args  {end.event  {}}  }  { 

# 

#  Initial  condition 

# 

set  conditionO  [ condition-procedure  args.Cff] 
if  $conditionO  then  { 

global  skill.name.e nd.event 
set  skill.name.e nd.event  $end_event 
add_c  [list  skilLname. statel  args.ll 
}  else  { 

echo  Sorry,  the  initial  conditions  for  executing  this  skill  are  not  given 

> 

return 


} 


11  args"  stands  as  a  place  holder  for  arbitrarily  many  arguments  that  the  procedure 
might  need,  for  instance  as  input  to  the  condition  procedures,  or  for  one  of  the 
following  states.  The  parameter  end.event  is  contained  in  any  atomic  skill. 

For  every  state  in  a  finite  state  machine  (refer  to  figure  3-2  on  page  24),  there  is 
a  separate  procedure  named  skilLname.sta.teM,  where  M  is  the  number  of  the  re¬ 
spective  state.  The  following  shows  the  general  structure  of  such  a  procedure  for  an 
intermediate  state: 


proc  skilLname.sta.teM  { args.M }  { 

# 

it  State  ending  condition 
# 

set  conditionMa  [ condUion.procedure.Ma  args.CMa ] 
set  conditionMb  tcondilion.procedure.Mb  args.CMb ] 
if  $conditionMa  then  { 

add.c  [list  skilLname. stateN  args.N] 
return  stop 

} 

if  $conditionMb  then  { 

add.c  [list  skilLname. stateP  args.P] 
return  stop 

} 

local.motor.programMl  by  angle  1  noren 
local.moior.programMS  by  angle  1  noren 


local.motor.programMn  by  angle  1  noren 
return 


} 
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Every  state  is  determined  by  several  state  ending  conditions,  and  a  number  of  local 
motor  program  procedures  that  cause  different  limbs  to  move  concurrently  while  a 
state  is  being  executed.  As  long  as  none  of  the  ending  conditions  of  a  state  is  true,  the 
state  remains  on  the  command  list  and  all  local  motor  program  procedures  belonging 
to  that  state  are  executed  once  every  step  through  the  command  loop. 

In  the  example  procedure,  the  state  M  branched  out  to  either  state  N  or  state  P, 
depending  on  whether  condition  Ma  or  condition  Mb  becomes  true.  Accordingly,  the 
state  N  or  state  P  procedure  will  be  added  to  the  command  list.  In  general,  a  state 
could  have  any  number  of  ending  conditions  (which,  by  themselves,  could  consist  of 
a  compound  of  different  subconditions).  Any  network  of  “and”  and  “or”  connections 
between  the  subconditions  can  make  up  a  state’s  ending  condition. 

If  a  state  procedure  returns  “stop”,  the  state  is  automatically  removed  from  the  com¬ 
mand  list.  Refer  to  the  source  code  of  the  server  procedure  that  runs  the  command 
loop  (page  136)  to  see  how  this  is  implemented. 

The  last  state  denoted  here  as  state  X  will  evaluate  the  end  event  upon  satisfac¬ 
tion  of  its  ending  condition  X,  but  might  branch  out  again,  if  other  conditions 
come  true.  This  possible  branching  is  not  included  in  the  example  procedure  for 
a  skilLnamest&teX : 


proc  skilLnamesta.teX  {arjs.A'}  { 

# 

#  State  ending  condition 

# 

set  conditionX  [condilion-procedure-X  args.CX ] 

if  SconditionX  then  { 

global  skilLname.e nd.event 
eval  [set  $£j7/_name_end_event] 
return  stop 

} 

locaLmotor.programXl  by  angle  i  noren 

locaLmotor„programX2  by  angle  1  noren 

locaLmoior.programXn  by  angle  1  noren 

return 

} 


Given  as  a  parameter  to  the  main  motor  program  procedure  was  the  end_event.  This 
parameter  is  made  accessible  to  the  last  state  X  by  being  defined  as  a  global  variable 
in  the  procedure  skilLname.  As  a  default,  the  end.event  is  set  to  “{}”  so  that  the 
evaluation  of  a  skill’s  end  event  will  have  no  effect.  The  procedure  then  goes  on  to 
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the  command  return  stop  which  forces  the  skill  to  end,  and  causes  the  last  state  to 
be  removed  from  the  command  list. 

There  are  cases  where  a  changing  environment  makes  it  necessary  to  perform  the 
same  skill  over  and  over  again,  because  once  the  skill  is  fulfilled,  the  initial  conditions 
might  come  true  again.  By  defining  the  skill-name  itself  as  end-event,  the  skill  s  first 
state  will  replace  the  skill’s  last  state. 

In  case  the  skill  consists  of  just  one  state  and  it  is  desired  to  execute  that  skill  again, 
any  time  the  initial  conditions  for  the  skill  are  satisfied,  the  end-event  has  to  be  set 
to  “return”.  The  procedure  will  then  return  before  it  reaches  the  command  return 
stop,  so  it  will  not  yet  be  removed  from  the  command  list.  This  way,  the  statel  for 
that  particular  skill  will  remain  on  the  list  until  it  will  get  removed  e.g.  when  another 
skill’s  end  event  comes  true. 

With  the  explained  method,  an  adaptive  reaching  algorithm  for  a  moving  object  can 
be  implemented.  Once  the  object  is  reached,  the  ending  condition  will  come  true,  but 
the  reaching  state  would  not  be  removed  from  the  command  list.  Any  time  the  object 
moves  farther  away  in  one  timestep  than  a  given  goal-point-precision,  the  reaching 
algorithm  would  be  activated  again. 

An  example  of  a  forward  kinematics  skill  put_arm_on_table  will  be  discussed  in  sec¬ 
tion  5.5. 


4.7.2  Motor  Programs  for  Atomic  Inverse  Kinematics  Skills 

Inverse  kinematics  skills  are  determined  by  an  inverse  kinematics  calculation  routine. 
They  consist  of  only  one  state  containing  all  local  motor  programs  involved  in  a 
specific  movement.  The  inverse  kinematics  calculation  routine  synchronizes  the  angle 
steps  of  all  these  local  motor  programs,  depending  on  how  many  timesteps  the  skill 
should  take  until  its  fulfillment. 

An  example  of  a  main  motor  program  procedure  for  an  inverse  kinematics  skill  is 
included  in  appendix  A  (page  107),  and  accounts  for  an  initial  condition  composed 
of  many  single  condition-procedures. 

Besides  the  parameter  end_event  which,  as  mentioned  earlier,  is  contained  in  any 
atomic  skill,  the  inverse  kinematics  skills  will  all  contain  the  parameters  time_opt 
and  timesteps.  If  the  parameter  time_opt  is  set  to  “1”,  the  time  when  the  execution 
of  the  skill  has  to  be  finished,  end_time,  has  to  be  calculated  depending  on  the  state 
of  the  internal  clock.  During  the  execution  of  the  IK-skilLnamesisXel  (see  appendix 
A,  page  108),  the  end_time  will  be  compared  with  the  internal  clock  ic,  and  the 
number  of  the  remaining  timesteps  will  be  set  accordingly.  It  will  be  guaranteed  that 


68 


Swetlana  Gaffron 


the  number  of  timesteps  never  drops  below  one,  even  though  the  end_time  for  the 
skill  might  be  exceeded. 

Usually,  and  as  a  default,  the  time.opt  parameter  will  be  set  to  “0”,  which  means 
that  the  remaining  number  of  timesteps  will  be  calculated  again  every  step  by  a 
timesteps-calculation-procedure  to  control  the  end  effector  or  joint  velocities..  For 
example,  the  procedure  calc_dist_timesteps  (see  appendix  A,  page  115)  calculates 
the  timesteps  necessary  to  overcome  a  certain  distance  depending  on  the  value  of  the 
global  variable  dist_per_timestep  that  specifies  the  average  distance  an  end  effector 
should  travel  per  timestep.  This  global  variable  can  be  changed  any  time,  even  during 
the  execution  of  the  skill,  using  the  procedure  dps. 

Again,  the  parameter  “args”  accounts  for  any  number  of  parameters  specific  to  a  skill 
(see  page  65). 

The  inverse.kinematics.calculation.procedure  first  calculates  the  final  absolute  joint 
angles  for  the  involved  limbs,  then  converts  the  absolute  angles  into  relative  joint 
angles  necessary  to  reach  the  final  goal  (depending  on  the  current  state  of  the  joints), 
and  finally  returns  the  relative  angle  steps  every  joint  has  to  move  during  the  next 
step.  The  local  motor  programs  will  only  be  activated  if  the  corresponding  angle  step 
size  is  greater  than  0.001. 


4.7.3  Motor  Programs  for  Composite  Skills 

Atomic  skills  can  be  combined  to  composite  skills  in  different  ways: 

1.  Parallel,  independent: 

Two  or  more  atomic  skills  are  added  to  the  command  list  at  the  same  time  so 
that  they  start  executing  synchronously.  Every  atomic  skill  executes  indepen¬ 
dently  from  the  others.  That  means  that  it  will  be  removed  from  the  command 
list  the  moment  it  satisfies  its  ending  conditions,  independent  of  the  duration 
of  the  other  atomic  skills  that  might  be  different.  See  figure  4-16  for  the  finite 
state  machine  model.  The  implementation  is  fairly  easy: 

proc  covipositeskilLname  {  args  }  { 
skilLnamel  args.SJ 
skilLnameS  args.S2 
return 


} 
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Figure  4-16:  Model  of  a  composite  skill  composed  of  parallel,  independent  atomic 
skills 

2.  Parallel,  dependent: 

As  in  the  former  case,  all  atomic  skills  will  start  executing  synchronously,  but 
in  this  case,  their  ending  conditions  are  coupled.  That  is  to  say,  all  atomic  skills 
corresponding  to  the  same  composite  skill  will  be  removed  from  the  command 
list  only  when  they  all  satisfy  their  corresponding  ending  conditions  at  the  same 
time  (see  figure  4-17).  To  couple  the  ending  conditions,  all  atomic  skills  but  one 
will  remain  on  command  list,  after  satisfying  their  individual  ending  conditions. 
One  of  the  atomic  skills  has  to  evaluate  the  combination  of  all  individual  ending 
conditions  and,  in  case  they  are  all  satisfied,  remove  the  atomic  skills  altogether 
from  the  command  list. 


Figure  4-17:  Model  of  a  composite  skill  composed  of  parallel,  dependent  atomic  skills 


Keeping  an  atomic  skill  on  the  command  list  can  be  realized  by  defining  the 
name  of  that  particular  atomic  skill  as  its  own  end.event  as  explained  earlier  on 
page  67.  This  way,  an  atomic  skill  will  add  its  first  state  again  to  the  command 
list  upon  completion. 

The  evaluation  of  all  ending  conditions  can  be  done  in  an  extra  procedure  de¬ 
fined  for  that  particular  purpose.  The  skill,  that  has  the  ending.condition^eval- 
uation.procedure  as  one  of  the  commands  of  its  end_event  removes  the  other 
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skills  directly  from  the  command  list  by  using  the  rem_c  procedure  and  will  be 
removed  itself  by  returning  “stop”. 


proc  composHe.skilLname  {  args  }  { 

skilLnamel  args.Sl  {skilLnamel  argsSl} 
skilLname2  args.S2  { skill.name2  args.S2} 
skilLnameS  args.S3  { 

ending. condition. evaluaiion.procedure 
rem_c  {skilLnamel*} 
rem_c  {skill.name2*} 

} 

return 

} 


3.  Sequentially: 

Feeding  another  skilLname2  as  end.event  to  a  skilLnamel  will  link  those  two 
skills  to  execute  sequentially. 


proc  composite.skill.name  {  args  }  { 
skilLnamel  args. SI  { 

skilLname2  args.S2  {} 

} 

return 

} 


Figure  4-18:  Model  of  a  composite  skill  composed  of  a  network  of  atomic  skills 


A  more  complex  network  of  atomic  skills  making  up  a  composite  skill  is  shown  in 
figure  4-18.  The  implementation  follows: 
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proc  composite.skilLname  {  args  }  { 
skilLnamel  args.Sl  { 

skilLname2  argsSS  {} 

IK.skilLnameS  argsSS  time.opt  timesteps  { 
skilLname4  args.S4  { 

skiU.name5  argsS5  {} 
skilLname6  argsSS  {} 

IK-skilLname7  args.S7  time.opt  timesteps  {} 

} 

} 

} 

return 

} 


Note  that  the  end  event  can  consist  of  a  list  of  commands.  In  this  way  it  is  possible 
to  start  several  atomic  skills  parallel,  upon  completion  of  a  previous  skill.  The  skills 
that  should  execute  sequentially  are  nested  as  each  other’s  end  events. 

In  this  section  it  has  been  shown  how  to  generalize  the  generation  of  motor  programs 
using  finite  state  machines.  Every  motor  program  consists  of  a  compound  of  tel 
procedures,  i.e.  one  for  each  state  and  a  main  motor  program  procedure  that  checks 
the  initial  conditions.  The  definition  of  each  state  consists  mainly  of  the  identification 
of  local  motor  programs  and  a  definition  of  the  ending  conditions  for  that  particular 
state.  Local  motor  programs  define  the  action  of  a  state  and  the  ending  conditions 
define  what  conditions  have  to  be  satisfied  in  order  to  proceed  to  the  next  state. 

A  distinction  has  been  made  between  forward  and  inverse  kinematics  skills.  It  has 
been  demonstrated  how  to  compose  more  complex  skills  from  atomic  skills  in  a  parallel 
or  sequential  manner,  or  a  combination  of  both. 

During  the  development  of  the  general  structure  of  motor  programs  it  became  clear 
that  there  is  a  variety  of  arguments  that  might  have  to  be  passed  to  specific  skills  and 
a  few  arguments  that  are  always  the  same,  e.g.  the  end.event  has  to  be  specified  for 
every  skill.  Other  arguments  might  be  the  goal.point,  i.e.  the  final  position  the  end 
effector  should  reach  in  a  skill  move_arm_to_goal  (as  will  be  shown  in  section  5.2.1). 

The  next  section  will  explain  more  about  condition  procedures  that  can  check  initial, 
as  well  as  ending,  conditions  and  introduce  some  of  them. 

Chapter  5  will  discuss  the  implementation  of  different  kinds  of  skills  represented  by 
motor  programs  according  to  the  generalization  worked  out  in  this  section. 
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4.8  Condition  Procedures 

Condition  procedures  are  boolean  procedures  that  check  whether  certain  conditions 
are  fulfilled  or  not  and  return  either  “1”  or  “0”  accordingly. 

Those  procedures  can  be  called  to  either  check  the  initial  conditions  for  a  skill  or  to 
check  the  ending  conditions  of  a  state  in  a  skill  in  order  to  decide  when  to  go  on  to 
the  next  state  and  which  state  should  be  executed  next  (refer  to  the  discussion  of 
the  motor  program  design  in  chapter  3.4.2).  In  general,  the  same  procedures  that 
function  as  ending  conditions  can  also  function  as  initial  conditions.  However,  it  has 
not  yet  been  attempted  to  explore  initial  conditions  specifically  since  this  will  later 
be  the  task  of  the  skill  network.  A  skill  will  then  only  be  invoked  by  the  skill  network 
if  the  initial  conditions  specified  in  the  skill  template  prove  to  be  given.  For  now  it 
will  be  taken  for  granted  that  the  initial  conditions  for  a  skill  are  satisfied. 


4.8.1  Classification  of  Condition  Procedures 

There  is  a  great  variety  of  ending  conditions.  Most  of  them  can  be  classified  in  the 
following  way,  where  conditions  1.  -  8.  represent  geometric  conditions  and  9.  and  10. 
conditions  that  do  not  imply  a  geometric  calculation1  : 

1.  Geometric  relationships  between  a  joint  and  an  object 
Comparing  the  location  of  a  joint  with  the  bounding  box  of  an  object. 

(a)  x joint  greater  than  xmax^>bj,  V joint  greater  than  ymax.obj ,  or  ZjQint  greater 
than  zinax _jribj  • 

(b)  X joint  less  than  £mt„_06j,  y joint  less  than  ymin.obj,  or  z joint  less  than  zmin_0bj. 

(c)  Joint  within  specified  distance  from  object. 

E.g.  “Is  the  wrist  under  the  table,  above  the  table  or  in  front  of  it?” 

2.  Geometric  relationships  between  a  body  part  and  an  object 
Comparing  the  bounding  boxes  of  a  body  part  and  an  object. 


xThe  following  notion  will  be  used: 

bounding  box  of  an  object  —  (Zmin^objj  ymin-obji  Zmin^obji  Zmax-obj  >  Vmax^obj  i  Zmax-obj  )i 
bounding  box  of  a  body  part  —  (Xmin-bp  t  Umin-bp i  Zmin^bpi  Zmax-bp i  Vmax^bp}  Zmax-bp') i 
joint  position  =  (xjoint,  yjdnt,  Zjoint),  and 
target  position  —  (^target)  ytargeti  Z target ) 
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(a)  Xmin_bp  greater  than  xmar_obj,  VminJbp  greater  than  ymax^>bj ,  or  zmin_bp  greater 
than  zmar_0bj  • 

(b)  xmaxjbp  less  than  xmin_0bj,  VmaxJbp  less  than  ymin.obj ,  or  zmaxj>p  less  than 

Zjnin-obj  • 

E.g.  “Is  the  glass  located  to  the  right  or  to  the  left  of  the  forearm?” 

3.  Geometric  relationship  between  joints 
Comparing  the  location  of  two  joints. 

(a)  Xj0inti  equal  Xj0int 2,  yjointi  equal  yj0int2  or  Zj0int  1  equal  Zj0int 2. 

(b)  x j0int\  less  than  x joint?,  y joint!  less  than  y joint?,  or  Zjointi  less  than  z joint?  ■ 

(c)  Xjointi  greater  than  Xj0int2,  yjointi  greater  than  y joint?,  or  Zjointi  greater  than 

Z joint? • 

E.g.  “Is  the  wrist  located  below  or  above  the  elbow?”  or  “Is  the  elbow  posi¬ 
tioned  to  the  right  or  to  the  left  of  the  shoulder  joint?” 

4.  Location  of  a  joint 

Comparing  the  location  of  a  joint  with  a  target  position. 

(a)  Xjoint  equal  Xtarget,  Vjoint  equal  ytarget,  °r  ZjQint  equal  Ztarget- 

(b)  Xjoint  less  than  xtarget,  y joint  less  than  ytarget,  or  Zjoint  less  than  Ztarget- 

(c)  Xjoint  greater  than  xtarget,  yjoint  greater  than  ytarget,  or  z joint  greater  than 
Ztarget- 

(d)  Joint  position  (.r joint,  yjoint ,  Zjoint )  equal  target  ( Xtarget ,  ytarget,  Ztarget) • 

(e)  Joint  position  within  specified  distance  of  object. 

E.g.  “Is  the  wrist  positioned  at  a  height  of  z=75  ?”  or  “Is  the  end  effector 
positioned  at  a  specified  goal  point?” 

5.  Orientation  of  a  body  part 

Measuring  the  angle  a  body  part  axis  includes  with  a  target  axis. 

(a)  Body  part  orientation  equal  target  orientation 

(b)  Body  part  orientation  aligned  with  a  specified  axis  of  an  object 

E.g.  “Does  the  forearm  point  in  the  target  direction?”  or  “Is  the  thumb  vector 
aligned  with  the  longitudinal  axis  of  the  glass?” 
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6.  Angular  constraints 

Comparing  a  joint  angle  with  a  target  angle. 

(a)  Joint  angle  equal  target  angle 

(b)  Joint  angle  greater  than  target  angle 

(c)  Joint  angle  less  than  target  angle 

(d)  Configuration  of  joint  angles  of  a  limb  equal  target  configuration 

E.g.  “Is  the  elbow  angle  less  than  130  degrees?”  or  “Is  the  arm  positioned  in 
the  target  configuration?” 

7.  Contact  between  a  body  part  and  an  object 
Checking  for  any  collision  between  a  body  part  and  an  object. 

E.g.  “Do  the  fingers  touch  the  table?”  or  “Does  the  left  toe  touch  the  ground?” 

8.  Contact  between  body  parts 

E.g.  “Does  the  palm  touch  the  lower  body?”  or  “Do  the  upper  right  and  the 
upper  left  leg  touch  each  other?” 

9.  Change  in  one  of  the  global  variables  of  the  SkillBuilder 
Checking,  for  example,  whether  the  arm_pose_parameter  changed  since  the  last 
timestep. 

10.  Ending  condition  of  another  skill  currently  being  executed  is  satisfied 

The  condition  mentioned  last  can  be  used  for  composite  skills  composed  of  parallel 
dependent  atomic  skills  (refer  to  section  4.7.3). 

In  many  cases,  a  condition  procedure  is  very  general  and  is  made  suitable  for  a 
specific  skill  state  by  passing  the  appropriate  arguments  to  the  procedure.  However, 
sometimes  condition  procedures  have  to  be  generated  that  are  very  specific  for  one 
skill.  Those  procedures  can  be  of  any  of  the  above  mentioned  kinds  and  will  have 
to  be  developed  at  an  ad  hoc  basis  when  defining  new  skill  states.  An  example  is 
given  for  the  2.  kind  of  condition  procedures  on  page  75  that  is  specifically  for  the 
reach_ceiling  skill. 


4.8.2  Implemented  Condition  Procedures 

So  far,  only  condition  procedures  needed  for  the  skills  introduced  in  this  thesis  have 
been  implemented. 


4.8  Condition  Procedures 
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Condition  1: 

Very  general  is  the  procedure 


joint_in_box  joint  object  option 


for  which  the  option  parameter  can  be  specified  as  “in”,  “x.plusJn”,  “y.plusJn”, 
“z_plusJn”,  “x.plus”,  “y.plus”,  “z.plus”,  “x_minusin”,  “y_minusin”,  “z_minusJn”, 
“x_minus”,  “y_minus”,  “z_minus”.  If,  for  example,  the  option  is  set  to  “z_plus_in” 
(see  figure  4-19),  the  procedure  checks,  whether  the  joint  is  located  above  the  object, 
but  inside  the  horizontal  bounding  square  of  the  object,  so  that  a  call  joint_in_box 
wrist  table  z_plus_in  is  only  true,  if  the  wrist  is  located  above  the  table  plane. 
If  it  only  plays  a  role,  whether  the  wrist  is  located  at  a  point  higher  than  the  table 
plane,  option  “z_plus”  should  be  used.  Option  “in”  tests,  whether  a  joint  is  located 
inside  the  bounding  box  of  an  object. 


Figure  4-19:  Option  “z_plus_in”  for  the  joint_in_box  condition  procedure 


Condition  2: 

The  condition  procedure 

hand_on_ceiling  ceiling-object 

is  true  when  the  highest  point  of  the  hand  is  located  above  the  lowest  point  of  an 
object  modeling  the  ceiling.  This  condition  procedure  is  already  quite  specific  since 
the  body  part  in  question,  i.e.  the  hand,  is  hardcoded.  In  the  case  of  the  hand  this 
is  necessary  because  the  compound  of  all  bounding  boxes  of  all  fingers  and  the  palm 
should  be  taken  into  account. 
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Conditions  4: 

The  following  condition  is  true  when  the  end  effector  (or  in  case  gp.opt  is  set  to 
“wrist”  the  wrist)  coincides  with  a  given  goal  point  within  the  goal_point_precision: 

goal-reached  goal-point  gp.opt 

The  goal_point_precision  is  a  global  variable  that  can  be  changed  any  time  using 
procedure  gpp. 

If  the  distance  between  a  goal  point  or  the  centroid  of  an  object  and  the  end  effector 
(or  in  case  gp.opt  is  set  to  “wrist”  the  wrist)  is  less  than  the  length  of  the  stretched 
out  arm,  the  following  conditions  are  satisfied: 

goal_in_reaching_distance  goal-point  gp.opt 
object_in_reaching_distance  object  gp.opt 


Conditions  5: 

Comparing  the  orientation  of  a  body  part  with  a  target  orientation  is  done  by  the 
procedures 

test  .hand-direction  axis.opt  target-axis 
test-f orearm.direction  target-axis 
test_upper_arra_direction  axis.opt  target-axis 


For  the  hand,  the  possible  axis.opts  are  “point”,  “palm”,  and  “thumb”,  specifying 
which  of  the  hand  axes  is  the  one  in  question.  The  upper  arm  will  generally  be 
used  with  an  axis.opt  “main”,  specifying  the  axis  from  the  shoulder  joint  to  the 
elbow.  However,  a  possibility  to  specify  “perp”  is  given,  where  “perp”  denotes  an 
axis  perpendicular  to  the  upper  arm’s  main  axis  in  the  plane  defined  by  the  upper 
arm’s  and  the  forearm’s  axes. 


Conditions  6: 

The  following  condition  procedures  check  whether  a  specified  joint  angle  configuration 
for  the  arm,  a  finger,  or  the  thumb  are  reached  by  comparing  the  sum  of  the  deviations 
of  the  current  configuration  from  the  target  configuration  with  the  global  variable 
angle-precision: 


4.8  Condition  Procedures 
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arm_conf ig_reached  config 
f inger.conf ig_reached  finger  config 
thumb _conf ig-reached  config 


The  target  configurations  are  passed  to  the  procedures  as  n  dimensional  vectors,  with 
n  =  9  for  the  arm,  n=3  for  a  finger  and  n=5  for  the  thumb.  Procedure 

arm_joint_angle_reached  joint-name  target-angle 

serves  to  check  whether  a  target  angle  for  a  specific  arm  joint  is  reached. 

Conditions  7: 

The  contact  between  a  body  part  and  an  object  is  detected  when  a  collision  takes 
place.  Since  there  was  no  collision  at  the  previous  timestep  and  the  stepsize  by  which 
the  body  parts  move  are  very  small,  the  collision  depth  is  also  very  small.  This  way  it 
is  allowed  to  define  the  “collision”  as  contact  between  the  body  part  and  the  object. 
The  following  procedures  have  been  provided: 

collision-fingers  object 
collision_lower_arm  object 
collision-fingertip  finger  object 
collision-hand  object 


The  procedure  collision^  ingers  tests  for  collision  of  each  link  of  all  fingers  with 
the  object.  The  collision-hand  procedure  checks  only  for  collision  of  the  hand  palm 
with  an  object. 

Conditions  9: 

Procedure 

sajne_posture 

checks  whether  the  arm_pose_parameter  changed  since  the  last  timestep  and 


reaching_problems 
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returns  the  boolean  value  of  the  global  variable  with  the  same  name.  This  variable 
might  have  been  set  to  “1”  during  the  execution  of  inverse  kinematics  calculation 
procedures  to  indicate  that  the  specified  goal  point  cannot  be  reached. 

4.8.3  Linking  several  Condition  Procedures 

An  ending  condition  can  be  composed  of  any  number  of  condition  procedures  by 
connecting  them  logically. 

For  example,  if  a  state  should  only  branch  to  state  X,  if  condition  1  and  condition  2 
and  (condition  3  or  condition  4)  are  true,  its  ending  condition  might  look  as  follows: 

set  condition_X  [list  [condition_procedure_l]  8:  [condition_procedure_2]  &  \ 

[list  [condit ion_proc edure_3]  I  [condition_procedure_4]  ]  ] 


Chapter  5 

Visually  Guided  Motion 


5.1  Orienting  a  Body  Part 

All  motor  programs  for  skills  to  orient  a  body  part,  e.g.  pointing  with  the  forearm 
in  a  specified  direction  or  aligning  the  thumb  axis  with  an  object  axis  are  based  on 
one  of  the  inverse-kinematics-calculation-procedures  introduced  in  chapter  4.5.  They 
belong  to  the  category  of  motor  programs  for  atomic  inverse  kinematics  skills  (see 
section  4.7.2). 

In  how  many  timesteps  the  target  orientation  will  be  reached  by  a  body  part  depends 
on  the  value  of  the  global  variable  angle_per_timestep  if  the  time.opt  parameter 
to  the  orient-body-part  skill  is  set  to  “0”.  A  time_opt  parameter  “0”  means  that 
the  number  of  timesteps  should  be  calculated  by  a  timesteps-calculation-procedure 
such  as  calc_angle_timesteps  (appendix  A,  page  114)  in  the  case  of  orient-body- 
part  skills.  If  the  time_opt  parameter  is  set  to  “1”  the  procedure  will  read  the 
number  of  timesteps  from  the  timesteps  parameter  instead  of  calculating  it.  The 
angle_per_timestep  can  be  changed  at  any  time  by  using  the  procedure  aps  (ap¬ 
pendix  A,  page  112). 

More  complicated  composite  skills  will  often  have  one  or  more  of  the  atomic  orient- 
body-part  skills  as  their  elements.  The  name  of  orient-body-part  skills  often  starts  with 
direct-***.  An  example  of  how  to  incorporate  the  direct_upper_arm,  direct-fore¬ 
arm  and  direct-hand  skills  in  a  skill  called  wave_hand  will  be  shown  in  section  5.6. 


5.1.1  Orienting  the  Upper  Arm 

The  atomic  skill  to  orient  the  upper  arm  along  a  given  vector  is 
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direct_upper_arm  main_axis  perp_axis  time.opt  timesteps  end_event 

The  main_axis  has  to  be  a  3  dimensional  vector  specifying  a  target  orientation  for 
the  upper  arm.  It  does  not  necessarily  have  to  be  a  unit  vector  since  the  inverse- 
kinematics-calculation-procedure  the  motor  program  is  based  on  (i.e.  upper_arm_an- 
gles_abs)  will  make  it  a  unit  vector.  The  perp.axis  parameter  is  usually  set  to  “no”, 
meaning  that  no  specific  perpendicular  axis  has  to  be  reached.  With  perpendicular 
axis  is  an  axis  denoted  that  is  perpendicular  to  the  main  axis  and  forms  together 
with  the  main  axis  the  plane  in  which  the  forearm  is  located.  In  case  a  perp.axis  is 
specified,  the  procedure  upper_arm_angles_abs  returns  not  only  an  arm-ls  and  arm-lf 
angle,  but  also  an  arm-turn  angle.  Those  angles  are  the  absolute  angles  needed  to 
reach  the  specified  orientation.  A  procedure  upper_arm_angles_rel  will  calculate  all 
differences  between  the  current  and  the  desired  joint  angles  and  divide  them  by  the 
number  of  timesteps  to  find  the  appropriate  stepsize  for  every  local  motor  program 
called,  i.e.  the  lift_arm_sideward,  the  lift_arm_f orward  and,  in  case  a  perp_axis 
is  specified,  the  turn_arm  procedures. 


5.1.2  Orienting  the  Lower  Arm 

The  orientation  of  the  forearm  can  be  changed  by  the  atomic  skill 

direct-forearm  main_axis  time_opt  timesteps  end_event 

which  is  based  on  the  f orearm_angles_abs  procedure  described  in  section  4.5.2 
(page  54).  The  stepsizes  for  both  local  motor  programs  involved,  i.e.  turn_arm 
and  bend_hand,  are  synchronized  in  procedure  f  orearm_angles_rel. 


5.1.3  Orienting  the  Hand  Axes 

For  orienting  the  hand  axes  there  are  three  different  skills  provided.  They  are: 

direct_hand_palm/point_axes  palm_axis  point_axis  time.opt  timesteps 

end_event 

direct  2h.and_palm_to wards  object  time_opt  timesteps  end.event 
align_hand_thumb_axis_with_obj ect  object  hand_bend_angle  time_opt 

timesteps  end-event 

The  direct _hand_palm/point_axes  skill  has  an  option  to  specify  both  a  target  palm 
axis  and  a  target  point  axis,  each  consisting  of  three  dimensional  vectors.  In  case 
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both  axes  are  given,  all  three  local  motor  programs  for  the  hand,  i.e.  turn_hand, 
bendJiand,  and  twist_hand,  will  be  activated.  In  case  there  is  no  point  axis  specified 
(by  setting  the  point-axis  parameter  to  “no”),  the  current  twist  angle  of  the  hand 
will  remain  the  same,  since  it  does  not  influence  the  palm  axis  (see  section  4.5.3, 
page  59). 

For  orienting  the  palm  axis  towards  an  object  the  skill  direct_hand_palm_towards 
is  provided  (see  appendix,  page  119).  It  is  very  similar  to  the  previous  one,  but  the 
palm  axis  will  be  computed  by  subtracting  the  current  end  effector  position  from  the 
centroid  of  the  specified  object.  No  hand  twisting  will  take  place.  This  skill  can  e.g. 
be  used  in  order  to  “look  at  a  picture”  if  the  picture  is  placed  in  the  hand  parallel 
to  the  palm,  the  palm  is  oriented  towards  the  head  and  the  head/eyes  are  oriented 
towards  the  picture. 

As  mentioned  earlier,  it  is  useful  for  some  grips  to  have  the  thumb  axis  aligned  with 
the  longitudinal  axis  of  an  object.  The  skill  alignJiand_thumb_axis_with-object 
therefore  first  calls  a  procedure  that  returns  a  vector  specifying  the  longitudinal  axis 
of  an  object,  given  the  object  (see  procedure  object_l_axis  included  in  appendix  A, 
page  133)  .  The  hand-turn  and  the  hand-twist  angle  are  calculated  by  the  procedure 
aligned_hand_angles_abs  discussed  on  page  57.  The  hand_bend_angle  is  provided 
as  an  input  parameter  to  the  skill. 


5.2  Reaching 

5.2.1  Simple  Reaching 

Like  the  orient-body-part  skills,  the  simple  reaching  skills  are  atomic  inverse  kinematics 
skills.  The  core  of  the  motor  programs  for  simple  reaching  consists  of  a  call  to  the 
goal_arm_angles_abs  procedure,  explained  in  detail  in  section  4.5. 

By  simple  reaching  it  is  meant  that  only  positioning  of  the  end  effector  (or  wrist) 
takes  place  without  orienting  the  hand  in  a  specific  direction,  and  there  is  no  collision 
avoidance.  The  following  motor  program  invokes  simple  reaching 

move_arm_to_goal  goal-point  gp_opt  time_opt  timesteps  end-event 

(see  appendix  A,  page  132),  by  changing  the  arm-ls,  arm-lf,  arm-turn,  and  elbow  joint 
angles  so  as  to  position  the  end  effector  at  the  specified  goal_point. 

The  procedure 


move_arm_to_object  object  gp.opt  time.opt  timesteps  end.event 
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is  capable  of  adapting  to  a  moving  object.  If  the  object  is  e.g.  a  ball,  the  orientation 
of  the  hand  in  respect  to  the  ball  does  not  play  a  role.  Thus,  applying  a  simple 
reaching  procedure  is  sufficient.  The  end  effector  is  moved  to  a  fictitious  point  on 
the  palm  vector  such  that  the  distance  between  the  new  end  effector  position  and  the 
palm  equals  the  radius  of  the  object.  This  way,  the  centroid  of  the  object  is  the  point 
the  end  effector  should  reach  for  in  order  to  achieve  proper  contact  between  the  palm 
and  the  object.  Every  timestep,  the  move_arm_to_object_statel  procedure  sets  the 
goal_point  to  the  current  centroid  of  the  ball  that  might  have  changed  meanwhile. 

The  goal_arm_angles_abs  also  recomputes  for  every  timestep  the  final  angles  neces¬ 
sary  to  reach  the  current  goal_point.  If  the  object  is  moving  faster  than  the  distance 
per  timestep  the  end  effector  is  supposed  to  travel,  the  end  effector  would  always  stay 
behind  the  object.  To  avoid  this  effect,  the  global  variable  dist_per_timestep  can 
be  set  to  a  greater  distance  by  using  the  procedure  dps  (included  in  appendix  A, 
page  121). 

As  for  the  orient-body-part  skills,  setting  the  time.opt  to  “0”  will  cause  the  number  of 
timesteps  to  be  calculated  by  the  appropriate  timesteps-calculation-procedure  which 
is  calc_dist_timesteps  in  that  case. 

5.2.2  Reaching  for  an  Object  with  Alignment  of  the  Hand 

Theoretically,  reaching  for  an  object  while  aligning  the  hand  along  the  objects  lon¬ 
gitudinal  axis  could  be  realized  as  a  composite  skill  composed  of  the  atomic  skills 
move_arm_to_object  and  align_hand_thumb_axis_with_object  executing  in  paral¬ 
lel.  When  aligning  the  thumb  axis  with  an  object,  the  absolute  hand  angles  needed 
to  bring  the  hand  in  the  desired  orientation  are  computed,  based  on  the  current  co¬ 
ordinate  frame  of  the  forearm,  located  at  the  wrist.  To  orient  the  hand  in  the  same 
way  based  on  a  different  coordinate  frame  of  the  forearm,  namely  the  one  after  the 
goal  point  is  reached  would  yield  different  hand  angles.  Since  the  hand  angles  would 
be  changed  only  by  the  amount  of  the  current  stepsizes  and  the  inverse  kinematics 
algorithms  are  recomputed  every  timestep,  the  composite  skill  would  eventually  bring 
the  joints  in  the  right  position. 

A  better  way  to  realize  such  a  reaching  skill  is  to  integrate  the  calculation  of  the  hand 
angles  with  the  calculation  of  the  arm  angles,  so  that  the  hand  angles  calculation  can 
be  based  on  the  final  arm  angles.  As  mentioned  earlier  in  section  4.5.4,  this  is  done 
in  procedure  reach_angles_abs. 

The  atomic  skill  based  on  the  reach  angles 

reach.object  object  gp_opt  hand_bend_angle  time_opt  timesteps  end_event 
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controls  seven  joint  motions:  arm  lifting  sideward,  arm  lifting  forward,  arm  turning, 
elbow  bending,  hand  turning,  hand  bending  and  hand  twisting,  where  the  hand  turn¬ 
ing  results  from  the  target  hand_bend_angle  that  was  given  as  a  parameter  to  the 
skill. 


5.2.3  Reaching  for  an  Object  along  a  Path 

In  order  to  avoid  collision  when  reaching  for  an  object,  a  collision  free  path  has  to  be 
generated  using  the  procedure  equidistant_path_points  explained  in  section  4.6. 
The  distance  between  one  point  on  the  path  and  the  following  point  will  always  be 
the  same  and  is  determined  by  the  value  of  the  global  variable  dist_per_timestep. 

The  end  effector  will  be  guided  through  each  of  the  path  points  except  the  last  one,  by 
linking  several  atomic  move_arm_to_goal  skills  sequentially.  One  of  the  equidistant 
path  points  will  be  passed  as  input  for  the  goal-point  parameter  to  each  of  these 
skills  in  order.  To  force  the  end  effector  to  reach  the  goal  point  in  one  timestep  the 
time_opt  has  to  be  set  to  “1”  and  the  number  of  timesteps  to  “1”.  So  far,  the 
hand  will  keep  its  original  joint  angles.  To  start  reaching  for  the  object,  (involving 
the  alignment  of  the  hand  thumb  vector  with  the  object),  a  dummy  object  is  placed 
at  the  location  of  the  last  path  point  before  the  object  is  reached,  and  assigned  the 
matrix  of  the  object  so  that  the  dummy  object  ends  up  in  the  same  orientation  as 
the  object  to  be  reached. 

Now,  the  atomic  skill  reach-object  dummy  will  be  defined  as  end  event  of  the  last 
move_arm_to_goal  skill,  and  reach-object  object  as  end  event  of  reach-object 
dummy  in  order  to  execute  all  the  named  skills  in  sequence. 

The  sequential  linking  of  the  atomic  skills  will  be  done  by  the  procedure  write.-  . 
reach-file  that  produces  a  file  called  reach-command  at  runtime  which  might  look 
as  follows: 

move_arm_to_goal_new  {7.87376884  -10.6772338  11.0305709}  ee  1  1  { 

move_arm_to-goal_new  {9.19729096  -9.05777862  11.0674169}  ee  1  1  { 
move_arm_to_goal_new  {10.5208131  -7.43832343  11.104263}  ee  1  1  { 

move-arm_to_goal_new  {11.8443352  -5.81886824  11.1411091}  ee  1  1  { 
move_arin_to_goal_new  {13.478763  -5.40810519  11.2134024}  ee  1  1  { 

move_arm_to_goal_new  {15.4184204  -6.18396817  11.3204958}  ee  1  1  { 
gPP  2 

reach-object  dummy  ee  -10  1  { 
gpp  0.4 

reach-object  glas  ee  -10  1  {} 

} 

} 

} 
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Finally,  calling  the  procedure 


reach_obj ect_along_path  object  hand_bend_angle  end_event 


will  cause  the  path  points  to  be  generated,  write  the  file  reach-command  (see  procedure 
write_reach_f  ile,  appendix  A,  page  139)  and  source  the  reach-command  file  so  as  to 
execute  the  composite  skill.  Figure  5-1  shows  the  hand  while  moving  the  end  effector 
along  the  posted  path  towards  a  glass  on  the  table.  Note  that  the  end  effector  is  set 
to  be  a  fictitious  point  1  inch  apart  from  the  palm. 


Figure  5-1:  Virtual  actor  performing  the  reach_object_along_path  skill 


5.3  Articulating  the  Fingers 
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5.3  Articulating  the  Fingers 

5.3.1  Grasping 

Grasping  should  only  be  invoked  when  the  object  to  be  grasped  is  reached.  The 
procedure 

grasp_object  object  speed  end_event 

is  implemented  as  a  composite  skill  invoking  the  atomic  skills 

close_finger  finger  object  bendl_step  bend2_step  end_event 
close-thumb  object  bendl_step  bend2_step  end_event 

for  all  fingers  in  parallel  (see  figure  5-2). 


Figure  5-2:  Composite  skill  grasp.object 


proc  grasp_object  {  object  {speed  ll  {end-event  {return  stop}}  1  { 
set  grasp_object_conditionO  [obj ect_reached  $object] 
set  bendl_thumb_step  $speed 
set  bend2_thumb_step  [mult  2  $speed] 
set  bendl_step  [mult  3  $bendl_thumb_step] 
set  bend2_step  [mult  3  $bend2_thumb_step] 
if  $grasp_object_conditionO  then  { 

close_finger  index  $object  $bendl_step  $bend2_step  {} 

close_finger  middlefinger  $object  $bendl_step  $bend2_step  {> 
close_finger  ringfinger  $object  $bendl_step  $bend2_step  $end_event 
close_finger  littlefinger  $object  $bendl_step  $bend2_step  {} 
close_thumb  $object  $bendl_thumb_step  $bend2_thumb_step  {} 
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>  else  •{ 

echo  Sorry,  the  initial  conditions  for  executing  this  skill  are  not  given! 

> 

return 

> 

The  local  motor  program  involved  in  the  close  .finger  atomic  skill  is 

bend_one_f inger_by  finger  bencLanglel  bend_angle2  timesteps  renopt 
and  for  the  close_thumb  atomic  skill 

bend_thumb_joint  joint  option  angle  timesteps  renopt 
Figure  5-3  shows  the  actor’s  hand  after  grasping  a  glass. 


5.3.2  Making  a  fist 

Making  a  fist  can  be  realized  by  defining  the  palm  itself  as  the  object  “to  be  grasped” 
and  invoking  the  same  atomic  skills  as  for  the  grasp_object  skill.  The  procedure  for 
that  skill  is 

make_fist  speed  end_event 


5.3.3  Changing  between  Different  Hand  Postures 

A  hand  posture  is  determined  by  a  specific  configuration  of  all  finger  and  thumb  joint 
angles.  There  are  several  composite  skills 

bring_all_f ingers_in_***_conf ig  timesteps  end_event 

where  ***  can  stand  for  “rest”,  “point”,  or  “open_grasp”  that  will  change  the  hand- 
posture  in  a  specified  number  of  timesteps  into  the  corresponding  configuration. 
The  point  position  is  shown  in  figure  5-4. 

The  skills  are  composed  of  the  atomic  skills 

bringjf  inger_in_conf  ig  finger  configuration  timesteps  end_event 
bring_thumb_in_conf ig  configuration  timesteps  end_event 


5.3  Articulating  the  Fingers 


Figure  5-3:  Hand  of  the  virtual  actor  grasping  a  glass 
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Figure  5-4:  Hand  of  the  virtual  actor  in  point  position 

that  are  to  be  executed  in  parallel  as  in  the  following  composite  skill: 

proc  bring_all_f ingers_in_point_conf ig  {{timesteps  4}  {end_event  {}}}  { 
set  bring_all_f ingers_in_point_conf ig_conditionO  [hand_free] 
if  $bring_all_f ingers_in_point_conf ig_conditionO  then  { 

bring_f inger_in_conf ig  index  {0  2  4}  $timesteps  {} 

bring_f inger_in_config  middlefinger  {10  80  90}  $timesteps  $end_event 

bring_f inger_in_conf ig  ringfinger  {-5  90  90}  $timesteps  {} 

bring_f inger_in_conf ig  littlefinger  {0  96  90}  $timesteps  {} 

bring_thumb_in_config  {-20  40  90  20  7}  $timesteps  {} 

}  else  { 

echo  Sorry,  the  initial  conditions  for  executing  this  skill  \ 
are  not  given! 

} 

return 

} 

Since  the  number  of  timesteps  for  all  atomic  skills  will  be  the  same,  the  end_event 
only  has  to  be  evaluated  by  one  of  them. 

The  atomic  skills  bringjf  inger_in_conf  ig  invoke  the  following  local  motor  pro¬ 
grams: 


bend_one_f inger_by  finger  bend_anglel  bend_angle2  timesteps  renopt 
spread_f inger  finger  option  spread.angle  timesteps  renopt 
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whereas  the  atomic  skill  bring_thumb_in_conf  ig  invokes  the  local  motor  programs 

spread-finger  thumb  option  spreacLangle  timesteps  renopt 
move_thumb_down  option  angle  timesteps  renopt 
turn_thumb  option  angle  timesteps  renopt 
bend_thumb_joint  1  option  angle  timesteps  renopt 
bend_thumb_joint  2  option  angle  timesteps  renopt 


5.4  Integrated  Reaching  and  Grasping 

Grasping  can  be  easily  integrated  in  the  composite  skill  reach_object_along_path 
(see  section  5.2.3)  by  adding  the  atomic  skill  bring_all_f  ingers_in_open_grasp_- 
conf  ig.  The  skill  to  open  the  fingers  should  start  executing  in  parallel  with  one  of  the 
move_arm_to_goal  skills  depending  on  how  many  timesteps  the  finger  opening  should 
take.  Defining  the  atomic  skill  grasp_obj  ect  as  end  event  will  make  the  fingers  grasp 
the  object  after  it  has  been  approached. 


5.5  Putting  the  Arm  on  the  Table 

The  following  skill  is  presented  as  an  example  of  an  atomic  forward  kinematics  skill 
modeled  by  a  complex  finite  state  machine,  according  to  figure  5-5.  If  the  wrist 


Figure  5-5:  Finite  state  machine  for  the  atomic  skill  put _arm_on_t able 
is  located  above  the  table  plane  (which  is  the  initial  condition  for  that  skill),  then 
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extending  the  elbow  -  as  will  be  done  in  state  1  of  the  finite  state  machine  -  will  make 
the  hand  approach  the  table  plane.  Depending  on  whether  the  forearm  or  one  of  the 
fingers  will  first  touch  the  table,  it  will  be  branched  to  either  state  2  or  state  3  of  the 
finite  state  machine.  State  2  makes  the  hand  bend  until  the  fingers  also  touch  the 
table  (this  is  the  ending  condition  of  the  skill).  In  case  the  fingers  touched  the  table 
first,  it  will  be  branched  to  state  3,  the  hand  will  be  bent  outwards  so  as  to  take  the 
fingers  away  from  the  table  until  they  no  longer  have  contact.  The  elbow  will  then 
extend  further  while  the  hand  will  still  be  bent  outwards  (state  4).  If  now  the  fingers 
touch  the  table  first,  states  3  and  4  will  be  repeated  until  the  condition  for  branching 
to  the  final  state  2  is  fulfilled,  namely  that  the  forearm  touches  the  table. 

The  put_arm_on_table  skill  does  not  include  any  collision  avoidance.  There  is  no 
visual  controller  that  would  guarantee  that  the  elbow  does  not  run  into  any  other 
body  link  or  object  in  the  virtual  environment  while  performing  the  skill.  Also,  the 
actor  would  not  “idealize”  if  there  is  an  object  placed  on  the  table  right  where  the 
hand  approaches  the  table  plane. 

All  procedures  defining  the  motor  programs  of  the  put _arm_on_t  able  skill  are  included 
in  appendix  A,  page  133. 


5.6  Waving  the  Hand 

Another  example  of  a  composite  skill  is  the 
wave_hand  end_event 

skill,  composed  mainly  of  orient-body-part  skills  (see  figure  5-6).  In  this  skill,  the  arm 
first  aims  specific  orientations  for  the  upper  arm  and  the  forearm.  Once  the  target 
orientation  for  the  upper  arm  is  reached,  the  hand  is  oriented  so  that  the  palm  axis 
points  forwards  (viewed  from  the  body).  The  target  for  the  forearm  orientation  alters 
between  two  orientations  while  the  hand  angles  are  constantly  adjusted  so  as  to  keep 
the  specified  palm  orientation. 

Hand  waving  is  an  oscillating  skill.  All  oscillating  skills  need  to  add  a  control  pro¬ 
cedure  to  the  command  list  (i.e.  wave_hand_control  in  this  case)  that  constantly 
checks,  whether  the  skill  should  keep  executing.  The  hand  waving  skill  can  be  stopped 
by  typing  stop_waving  (in  the  Do  Command  once  textfield).  This  will  set  the 
global  variable  stop_waving  to  “1”  so  that  the  wave_hand_control  procedure  will  re¬ 
move  all  atomic  skills  invoked  by  wave_hand  from  the  command  list,  including  itself. 
Furthermore,  it  will  evaluate  the  end  event  of  wave_hand. 
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Figure  5-6:  Composite  skill  wave_hand 


This  way  it  is  possible,  to  bring  the  arm  back  in  its  original  position  by  defining  the 
skill 

move_arm_to_conf  ig  config  time_opt  timesteps  end_event 

as  end  event  of  waveJiand.  The  parameter  config  could  be  set  to  “$org”  which  is 
the  global  variable  in  which  the  original  arm  configuration  is  stored.  The  call  would 
look  as  follows: 

wave_hand  f 

global  org 

move_arm_to_conf ig  $org  0  0  {> 

} 


5.7  Head/  Eye  Controller 

A  controller  for  the  head  and  eye  movements  has  been  developed  by  Sunil  Singh 
and  his  student  Dan  Popa  at  Dartmouth  College  and  has  been  integrated  in  the 
SkillBuilder  [9]. 

It  is  the  task  of  the  head-eye  controller  to  track  a  target  object  moving  in  a  3- 
dimensional  space.  The  controller  has  been  implemented  as  a  manipulator  written 
in  C,  communicating  with  the  tel  procedures  of  the  SkillBuilder  through  a  series  of 
ppread  and  ppwrite  commands. 

The  head,  represented  by  a  model  of  a  skull,  is  placed  on  the  neck  at  joint  9  of 
the  dhchain  of  the  spine  and  is  capable  of  limited  motion  about  three  intersecting 
coordinate  axes,  like  a  spherical  wrist  in  robot  manipulators.  The  eyes  are  represented 
by  two  white  balls.  Two  smaller  blue  balls  inside  the  white  balls  represent  the  pupils. 
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Singh  and  Popa  make  use  of  the  fact  that  a  target  can  be  characterized  by  its  coordi¬ 
nates  with  respect  to  a  fixed  frame  and  by  an  angle  a,  which  represents  the  angular 
orientation  of  the  object  with  respect  to  the  rotated  axis  normal  to  the  skull.  They 
determine  the  rotation  matrix  of  the  skull  relative  to  the  fixed  frame  by  using  the  Eu¬ 
ler  formalism,  and  solve  the  formulated  inverse  kinematics  equations  for  the  rotation 
angles  around  the  fixed  frame. 

Since  it  is  not  intended  to  move  the  head  towards  a  target  at  all  times,  but  rather 
move  only  the  eyes  or  both,  they  make  the  following  considerations: 

®  The  fovea  of  both  eyes  can  be  independently  directed  to  the  object  of  interest 
( vergence  eye  movements). 

9  If  the  object  is  “almost”  in  front  of  the  eyes,  the  eyes  move  only  in  a  quick, 
gaze-shifting  response,  called  a  saccade. 

•  Both  the  skull  and  the  eyes  have  a  relative  rotation  range  which  cannot  be 
exceeded. 

®  If  the  object  to  be  tracked  is  moving  outside  of  the  range  for  “comfortable”  eye 
movements,  the  head-eye  motions  must  be  carefully  distributed  between  the 
head  and  the  eyes. 

While  the  head  control  unit  uses  purely  inverse  kinematics,  the  eye  control  unit  ex¬ 
plicitly  accounts  for  the  dynamics  of  the  object  and  the  human  system. 

The  SkillBuilder  provides  the  following  atomic  skills  to  provoke  head/eye  motions: 

head-track  object  end_event 

head-straight  tirae_opt  timesteps  end.event 

head_move  object  time_opt  timesteps  end.event 


Each  of  those  skills  will  put  the  appropriate  procedures  on  the  command  list  and  will 
be  executed  only  if  the  command  loop  is  running.  The  headjnove  skill  will  cause  the 
head  and  eyes  to  move  the  focus  towards  the  specified  object  in  the  given  number 
of  timesteps  and  evaluate  its  end.event  upon  completion  of  the  skill.  The  skill  will 
be  able  to  adapt  to  a  moving  object. 

Keeping  the  focus  of  the  head  and  eyes  on  a  moving  object  can  be  done  by  invoking 
the  head-track  skill.  It  is  useful  to  define  head-track  as  end  event  of  head_move, 
in  order  to  slowly  move  the  focus  towards  an  object  before  tracking  it.  head-track 
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is  a  continuous  skill  which  means  it  will  only  stop  executing  when  it  is  explicitly 
stopped  by  calling  the  procedure  stop_head_track.  Remember  that  while  the  com¬ 
mand  loop  is  running,  interactions  are  only  possible  through  the  menus.  This  means 
stop_head_track  has  to  be  typed  in  the  Do  Command  Once  text  field. 

The  skill  head-straight  is  nothing  else  but  a  call  to  the  head_move  skill  with  a 
dummy  object  placed  at  the  point  the  head  should  look  at. 

Figure  5-7  shows  the  virtual  actor  with  the  skull  looking  at  a  ball  in  its  hand. 


Figure  5-7:  Virtual  actor  looking  at  a  ball  in  its  hand 
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Chapter  6 
Future  Work 


The  presented  state  of  the  SkillBuilder  is  prototypical.  In  this  chapter  there  will 
be  suggestions  on  how  to  extend  the  functionality  of  the  SkillBuilder,  improve  the 
performance  and  make  it  more  user-friendly. 

Extensions  of  the  functionality  of  the  SkillBuilder  could  be  made  by: 

•  Articulating  the  rest  of  the  limbs 

So  far,  the  virtual  actor  has  kinematic  chains  defined  for  the  spine,  the  left  arm, 
all  fingers  and  the  thumb  of  the  left  hand,  as  well  as  for  the  left  leg  and  its 
foot.  For  skills  involving  both  arms  and/or  legs,  however,  the  model  needs  to 
be  extended  to  include  kinematic  chains  for  the  right  arm,  all  fingers  and  the 
thumb  of  the  right  hand,  as  well  as  for  the  right  leg  and  foot.  The  process  of 
finding  the  right  Denavit-Hartenberg  parameters  might  be  a  bit  tricky,  given 
that  the  notation  does  not  allow  “mirroring”.  But  looking  at  the  parameters  of 
the  dhchain  for  the  left  limbs  of  the  actor  already  provided,  and  using  the  limb 
edit  menu  will  be  very  helpful. 

•  Creating  local  motor  programs  for  all  articulated  joints 

Local  motor  programs  exist  only  for  the  left  arm  and  all  fingers  and  the  thumb 
of  the  left  hand.  Creating  local  motor  programs  for  all  other  joints  will  be  trivial 
given  the  templates  from  the  implemented  ones  (see  section  4.4.2). 

•  Extending  the  list  of  condition  procedures 

The  classification  of  condition  procedures  given  in  section  4.8.1  suggests  several 
general  conditions  that  have  not  yet  been  implemented.  As  the  complexity  and 
variety  of  skills  increases  more  condition  procedures  appropriate  for  those  skills 
will  have  to  be  generated. 
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®  Developing  a  range  of  representative  motor  skills 
The  range  of  representative  motor  skills  should  be  extended  to  match  the  task 
primitives  mentioned  in  section  3.3. 

®  Creating  of  a  more  complex  virtual  world  for  the  actor 

The  more  complex  the  skills  get  the  virtual  actor  should  perform  the  more  there 
will  be  a  need  for  different  kinds  of  objects  in  the  environment,  e.g.  a  chair  he 
could  sit  on,  a  door  he  could  open,  different  objects  on  the  table  he  could  pick 
up  and  lift,  and  so  on. 

Improvements  to  the  performance  of  the  motor  skills  are  possible  by: 

©  Integrating  a  more  general  collision  avoidance  and  collision  detection 
feature 

There  is  much  current  research  in  finding  the  best  collision  free  path  through 
a  cluttered  environment.  Especially  in  robotics,  many  algorithms  for  a  manip¬ 
ulator  with  several  links  have  been  suggested.  As  the  virtual  environment  for 
the  actor  gets  more  and  more  complex,  it  will  be  important  to  integrate  a  more 
general  path  finding  algorithm  and  possibly  extend  it  with  a  collision  detection 
feature.  It  will  be  necessary  not  only  to  guarantee  a  collision  free  path  for  the 
end  effector,  i.e.  the  palm,  but  also  all  links  of  a  moving  limb. 

9  Including  dynamics 

As  the  fidelity  requirements  of  the  applications  increase  it  will  be  necessary  to 
include  dynamics  in  the  simulation  to  make  it  look  more  realistic. 

®  Confirming  the  performance  with  experimental  data 

The  movements  of  the  actor  performing  a  skill  could  be  compared  with  experi¬ 
mental  data  from  human  subjects  performing  the  same  tasks. 

®  Speeding  up 

By  distributing  computational  processes  over  a  network  of  machines  the  perfor¬ 
mance  could  be  made  faster  and  made  look  more  realistic. 

Improvements  of  the  user  friendliness  of  the  SkillBuilder  could  be  achieved  by: 

®  Developing  an  automated  motor  program  builder 

It  would  be  nice  if  the  skill  designer  would  not  have  to  write  tel  source  code. 
Because  the  general  structure  of  motor  programs  has  been  defined  in  this  thesis, 
it  will  easily  be  possible  to  generate  a  motor  program  builder  for  atomic  as 
well  as  composite  skills.  A  motor  program  builder  would  prompt  the  user  for 
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all  necessary  information  to  create  the  source  code,  i.e.  the  motor  programs, 
automatically. 

•  Providing  a  graphical  user  interface 

This  is  certainly  one  of  the  most  important  issues  to  make  the  SkillBuilder 
easier  to  handle.  Once  the  generation  of  motor  programs  has  been  automated, 
a  visual  programming  language  for  defining  motor  skills  should  be  implemented 
as  described  in  section  3.4.3  (the  ultimate  SkillBuilder  interface).  Furthermore, 
all  available  skills  should  be  able  to  invoked  by  choosing  an  item  from  a  graphical 
list,  taking  its  arguments  from  another  graphical  menu.  The  values  of  all  global 
parameters  should  be  displayed  and  able  to  be  changed  by  graphical  means. 
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Chapter  7 
Conclusion 


This  thesis  describes  the  development  of  the  SkillBuilder,  a  software  system  that  can 
function  as  a  design  tool  for  motor  programs  -  the  source  code  of  skills  a  virtual  actor 
can  perform.  The  implementation  was  done  in  the  3d  virtual  environment  simulation 
system  explained  in  the  thesis. 

A  human  figure  model  was  generated  consisting  of  rigid  body  parts  that  are  linked 
together  as  several  kinematic  chains  using  the  Denavit-Hartenberg  notation.  Every 
joint  of  the  figure  can  be  articulated  by  a  corresponding  local  motor  program. 

A  classification  of  skills  into  atomic  and  composite  skills  has  been  made  where  the 
atomic  skills  were  further  classified  into  forward  and  inverse  kinematics  skills.  Atomic 
skills  are  modeled  by  finite  state  machines  that  define  different  states  to  be  run 
through  during  the  execution  of  a  skill,  as  well  as  their  transition  conditions.  The 
composition  of  more  complex  skills  into  composite  skills  has  been  demonstrated  by 
linking  atomic  skills  in  a  parallel  or  sequential  manner,  or  in  a  combination  of  both. 

It  has  been  shown  how  to  generalize  the  generation  of  motor  programs.  Every  motor 
program  consists  of  a  compound  of  procedures,  each  defining  one  state  of  a  finite  state 
machine.  A  state  mainly  consists  of  the  identification  of  local  motor  programs  (i.e. 
the  action  to  be  taken  during  the  execution  of  that  particular  state)  and  a  definition 
of  the  ending  conditions  (i.e.  conditions  that  have  to  be  satisfied  in  order  to  proceed 
to  the  next  state). 

Ending  conditions  have  been  classified  and  a  selection  of  condition  procedures  have 
been  implemented.  In  many  cases,  a  condition  procedure  is  very  general  and  is 
made  suitable  for  a  specific  skill  state  by  passing  the  appropriate  arguments  to  the 
procedure.  However,  sometimes  condition  procedures  have  to  be  generated  that  are 
very  specific  for  one  skill.  Those  kind  of  procedures  will  have  to  be  developed  on  an 
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ad  hoc  basis  when  defining  new  skills. 

The  problem  of  parallel  execution  of  several  motor  programs  has  been  addressed 
by  introducing  the  command  loop:  an  endless  loop  that  constantly  evaluates  all 
procedures  currently  on  the  command  list.  Thus,  every  motor  program  currently 
executing  has  an  entry  of  one  of  its  states  in  the  command  list. 

The  focus  of  the  work  was  on  visually  guided  reaching  and  grasping  skills  using  the 
left  arm. 

Reaching  for  an  object  involves  inverse  kinematics  calculations.  The  algorithm  for 
positioning  the  palm  at  a  specified  point  in  space  takes  four  joint  angles  into  account, 
the  two  angles  of  the  spherical  joint  and  the  shoulder,  the  elbow  angle  and  the  arm- 
turn  angle.  The  first  two  of  these  angles  determine  the  upper  arm  direction.  The 
arm-turn  angle  is  the  angle  the  forearm  is  turned  about  the  upper  arm  axis.  The 
additional  degree  of  freedom  is  used  to  influence  the  relative  elbow  position  which 
determines  the  rotation  of  the  arm  about  the  vector  from  the  shoulder  to  the  goal 
point.  It  is  guaranteed  that  the  elbow  always  stays  outside  of  the  plane  defined  by 
the  upper  arm  and  the  forearm  in  the  lowest  possible  elbow  position,  as  viewed  from 
the  center  of  the  body.  The  three  angles  of  the  hand  are  calculated  from  a  target 
orientation  the  hand  should  finally  reach.  For  the  course  of  this  thesis  only  a  power 
grip  has  been  considered  for  which  the  thumb  axis  (i.e.  an  axis  in  the  palm  plane 
pointing  towards  the  thumb)  needs  to  be  aligned  with  the  longitudinal  axis  of  the 
object  to  be  grasped. 

Adaptation  to  a  moving  object  or  a  changing  shoulder  position  (when  lifting  the 
shoulder,  when  leaning  forwards,  or  even  when  moving  the  whole  body  while  reaching) 
is  achieved  by  recomputing  the  inverse  kinematics  calculations  every  timestep. 

The  global  variable  arra_pose_parameter  contains  a  measurement  for  the  relative 
elbow  position  that  can  be  changed  at  any  time.  Several  other  global  parame¬ 
ters  like  the  dist_per_timestep,  angle_per_timestep,  goal_point_precision  and 
angle_precision  have  also  been  defined  and  can  be  changed  any  time  duiing  the 
execution  of  the  skills  in  order  to  alter  the  performance. 

A  collision  avoidance  algorithm  specific  to  the  left  arm  has  been  implemented  that 
guarantees  that  the  hand  will  move  from  its  current  position  to  the  object  without 
colliding  with  a  specified  collision  object,  i.e.  the  table.  However,  the  algorithm  does 
not  yet  guarantee  collision  free  moves  of  the  other  arm  links,  i.e.  the  upper  arm  and 
the  forearm.  In  order  to  avoid  collisions  of  these  arm  links  the  relative  elbow  position 
can  be  influenced  appropriately  by  changing  the  arm_pose_parameter. 

The  reaching  and  grasping  skills  have  been  successfully  demonstrated  as  leal-time 
response  to  a  direct  interaction  between  the  virtual  environment  participant  and  the 
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virtual  actor.  A  glass  positioned  on  a  table  located  right  in  front  of  the  virtual 
actor  can  be  “picked  up”  and  positioned  anywhere  else  using  a  dataglove  that  has 
been  hooked  up  to  the  SkillBuilder.  When  “asking”  the  actor  to  grasp  the  glass,  an 
appropriate  collision-free  path  is  generated  and,  if  desired,  shown  on  the  screen.  The 
actor  starts  forming  an  open  grasp  position  with  the  fingers  while  reaching  for  the 
glass  along  the  collision  free  path.  Once  the  glass  has  been  reached  the  actor  wraps 
the  fingers  around  the  glass  until  a  contact  has  been  detected  between  each  of  the 
fingers  and  the  glass.  When  “picking  up”  the  glass  again,  the  actor  may  be  asked 
to  hang  on  to  it.  The  actor  always  adjusts  the  angles  of  the  hand  so  that  it  keeps  the 
same  orientation  with  respect  to  the  glass.  This  way  it  is  also  possible  to  demonstrate 
a  drinking  actor  (see  figure  7-1). 

The  focus  was  deliberately  put  on  visually  guided  reaching  and  grasping  skills,  rather 
than  on  the  attempt  to  implement  a  range  of  behaviors.  The  exercise  has  led  to  a 
better  understanding  of  the  issues  involved  in  designing  the  needed  software  tools 
that  will  serve  to  extend  the  SkillBuilder  with  a  range  of  representative  motor  skills. 

Validating  the  implemented  motor  programs  with  human  clinical/experimental  data 
is  left  for  future  work. 
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Figure  7-1:  Drinking  actor 
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Appendix  A 


General  Structure  of  Inverse 
Kinematics  Skills 


proc  IKskilLname  {  args  {time_opt  0}  {timesteps  1}  {end_event  {}}  }  { 
# 

#  Initial  condition 

# 

set  conditionO  [list  [ condition-procedureA  args]  ft  \ 

Icondition.procedureB  args]  ft  \ 

[ condilion.procedureN  args]  ] 

if  $conditionO  then  { 

global  IK-S kilLname-e nd_e v e nt 

set  JA'_sfa7Lname_end_event  $end.event 


global  ic 

set  $end_time  [plus  $ic  $tiraesteps] 

add_c  [list  JK-skilLname.sta.tel  args  $tirae_opt  $end_time] 

}  else  { 

echo  Sorry,  the  initial  conditions  lor  executing  this  skill  are  not  given 

} 

return 

} 
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proc  IK.skilLnamesta.tel  {args  time_opt  end.time  }  { 

# 

#  State  ending  condition 

# 

set  conditionl  [list  Icondition.procedureA  argsl  k  \ 

[ condition.procedu.reB  argsl  k  \ 

[ condition.procedureN  arjs]] 

if  $conditionl  then  { 

global  IK.skill.name.end.ewent 
eval  [set  IK.skill.name.end.eventl 
return  stop 

if  {  $time_opt  ==  0  }  then  { 

set  timesteps  Itimesleps.calculalion.procedure  argsl 
else  { 

global  ic 

set  timesteps  [minus  $end-time  $ic] 
if  [lesseq  $timesteps  0]  then  { 
set  timesteps  1 

} 

set  joint-angles  [ inverse.kinematics.calculation.procedure  args  $timesteps  ] 
set  joint_angleO  [lindex  $joint_angles  0] 

set  joint_angleN  [lindex  $joint_angles  N] 
if  [greater  [abs  $joint_angleO]  0.001]  then  { 
local.motor.programO  by  angle  1  noren 

} 

if  [greater  [abs  $joint_angleN]  0.001]  then  { 
local.motor.programN  by  angle  1  noren 

} 

return 


} 
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Source  Code  Examples 


*###############################*#########*######*##***#####*########«# 

9  add_c 

99999999999999999999999999999999999999999999999999999999999999999999999 

proc  add_c  {comm}  { 

global  commandlist ; 
lappend  commandlist  Icon* 
return 


99999999999999999999999999999999999999999999999999999999999999999999999 

9  align_hand_angle_abs 

99999999999999999*99999999999999999999999999999999999999999999999999999 

proc  aligned_hand_angles_abs  {  thumb.axis  hand_bend_angle  {M10  dhlO}  }  { 

set  thumb_axis  [div  $thumb_axis  [enorm  $thumb_axis]] 

if  {[string  compare  $H10  "dhlO"]  ==  0}  then  { 
set  H10  [dhmatrix  10  /l_arm/dhc] 

> 

Hset  $H10 
Hinvert 
Mele  300 
Hele  310 
Hele  320 

set  T  [Mxform  [mult  -1  $thumb_axis]] 
set  TO  [lindex  $T  0] 
set  T1  [lindex  $T  1] 
set  T2  [lindex  $T  2] 

9999999999  thetal3  is  given  in  form  of  hand.bend.angle  #######»## 

set  thetal3  $hand_bend_angle 
set  sl3  [sin  $thetal3] 
set  cl3  [cos  $thetal3] 

############  Calculation  of  thetal4  ############ 
set  sl4  [div  [mult  -1  $T2]  $cl3] 
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#  asin  yields  in  ”90  <=  thetal4  90 

t  The  other  solution  doesn’t  have  to  be  evaluated,  since  the 

#  allowed  range  for  thetal3  is  -40  to  40  degrees, 
set  thetal4  [asin  $sl4] 

set  ci4  [cos  $thetal4] 

##########**  Calculation  of  thetaii  ###########• 

set  sll  [expr  {($Tl*$cl4  +  $T0*$T2*$sl3/$cl3)  /  ($T0*$T0  +  $T1*$T1)>] 

#  asin  yields  in  -90  <=  thetall  <-  90 
set  thetalla  [asin  Jsll] 

if  {Jthetalla  <=  -80}  then  {set  thetalla  [plus  360  $thetalla]> 

t  90  <=  thetall  <=  270 

set  thetallb  [minus  180  $thetalla] 

t 

t  Find  out,  which  one  of  the  two  solutions  for  thetall  is  the  right  one 

t 

S  case  1  (thetall)  : 

set  M14  [calc_H14_from_H10_ll_13_14  $H10  $thetalla  $thetal3  $thetal4] 
set  easel  thumb.axis  [mult  -1  [list  [lindex  $H14  4]  \ 

[lindex  $H14  S]  [lindex  $H14  6]]] 
set  angle.diffl  [abs  [vangle2  $casel_thumb_axis  $thumb_axis]] 
if  [less  $ angle.diffl  0.1]  then  { 
set  thetall  $thetalla 
}  else  { 

S  case  2  (thetall)  : 

set  H14  [calc.H14.from_H10.11.13.14  $H10  $thetallb  $thetal3  $thetal4] 
set  case2  thumb.axis  (mult  -1  [list  [lindex  $H14  4]  \ 

[lindex  $M14  S]  [lindex  $K14  6]]] 
set  angle_diff2  [abs  [vangle2  $case2_thumb.axis  $thumb_axis]] 
if  [less  $angle_diff2  0.1]  then  { 
set  thetall  $thetallb 
}  else  { 
t 

t  case  3  (thetall)  :  -• 

•  Take  the  best  possible  solution. 

* 

if  {[less  $ angle.diffl  $angle_diff2]}  then  { 
set  thetall  $thetalla 
>  else  { 

set  thetall  $thetallb 

y 

y 

> 

set  tla.angle  [minus  Sthetall  90] 

set  bh.angle  $thetal3 

set  th.angle  [mult  -1  $thetal4] 

return  [list  $tla_angle  $bh. angle  $th. angle] 


###########*####################*##**###*****#************************* 
t  align_hand-palm/point.axis 

################################****«*#**##*#************************** 

proc  align.hand.palm/point.axis  {palm.axis  {point.axis  no}  \ 

{time.opt  0}  {timesteps  4}  {end.event  {}}  }  { 

* 

*  Check  whether  the  initial  conditions  are  fulfilled 
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i  and  put  the  command  on  the  list 

« 

set  conditionOa  1 

if  {  JconditionOa}  then  { 

global  align_hand_palm/point_axis_end_event 

set  align_hand.palm/point_axis_end. event  $end_event 

global  ic 

set  end.time  [plus  $ic  $timesteps] 

add.c  [list  align.hand.palm/point_axis_statel  Jpalm.axis  $point_axis  $time_opt  Jend.time] 
}  else  { 

echo  Sorry,  the  initial  conditions  for  executing  this  skill  are  not  given! 

> 

Com_Update_cb 

return 

> 

#################################*#########*###**#*###*#####******»**** 

*  align_hand_palm/point_axis_statel 

####»*##«####**####### 

proc  align_hand_palm/point_axis_statel  {palm.axis  point_axis  time_opt  end.time}  { 

* 

*  State  ending  condition 

» 

if  {[llength  Jpoint.axis]  ==  3}  then  { 

set  align_hand_palm/point_axis_conditionl  \ 

[list  [test_hand_direction  point  $point_axis]  \ 
t  [test_hand_direction  palm  Jpalm.axis]] 

>  else  { 

set  align.hand.palm/point.axis.conditionl  [test.hand.direction  palm  $palm_axis] 

} 

if  ${align_hand_palm/point_axis_conditionl}  then  { 
global  align_hand_palm/point_axis_end_event 
eval  ${align_hand_palm/point_axis_end_event}- 
return  stop 

> 

if  {$time_opt  ==  0}  then  { 

if  {[llength  $point_axis]  ==  3}  then  { 

set  tsl  [calc.angle.timesteps  hand  point  $point_axis] 
set  ts2  [calc.angle.timesteps  hand  palm  $palm_axis] 
set  timesteps  [max  $tsl  ts2] 

}  else  { 

set  timesteps  [calc.angle.timesteps  hand  palm  $palm_axis] 

> 

}  else  { 

global  ic 

set  timesteps  [minus  $end_time  $ic] 
if  {[lesseq  Jtimesteps  0]}  then  { 
set  timesteps  1 

> 

> 

set  hand_angles  [hand_angles_rel  $palm.axis  $point_axis  Jtimesteps] 
if  [equal  Jhand.angles  999]  then  { 

echo  Sorry,  cannot  position  the  hand.palm  axis  in  desired  direction, 
return  999 

> 

set  tla.angle  [lindex  Jhand.angles  0] 
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set  bh.angle  [lindex  {hand.angles  1] 

set  th.angle  [lindex  $hand_angles  2] 

if  [greater  [abs  {tla.angle]  0.0001]  then  { 
turn.louer.arm  by  {tla.angle  1  noren 

> 

if  [greater  [abs  {bh.angle]  0.0001]  then  { 
bend  hand  by  {bh.angle  1  noren 

> 

if  [greater  [abs  {th.angle]  0.0001]  then  { 
tuist.hand  by  {th.angle  1  noren 

> 

return 

} 


############«###########*#########»###########################»#####*## 
t  aps 

#######################»#####################*#######*######»# ###««#### 

proc  aps  {{angle  {»}  { 

global  angle.per.timestep 

if  {$angle  ==  then  { 

echo  angle.per.timestep:  {angle.per.timestep 
>  else  { 

set  angle.per.timestep  Wangle 

} 

return  {angle.per.timestep 

> 


################################ #####»####*###############*# 

t  app 

proc  app  {{para  {»>  { 

global  arm.pose. parameter 
global  arm.pose.opt 

if  {$para  ==  "">  then  { 

if  {$arm_pose_opt  ==  0>  then  { 
global  arm.pose.default 

echo  arm.pose. parameter:  Jarm.pose.def ault  (default  value) 

}  else  { 

echo  arm.pose.parameter :  $arm_pose_parameter 

> 

}  else  { 

if  {$para  ==  "default"}  then  { 
global  arm.pose.default 
set  arm.pose.opt  0 

set  arm.pose.parameter  $arm_pose_default 
echo  arm.pose.parameter:  {arm.pose.parameter 
}  else  { 

set  arm.pose.opt  1 
if  {{para  <  0}  then  { 

echo  Attention:  trying  to  set  an  arm.pose.parameter  <  0 
set  arm.pose.parameter  0 
}  else  { 
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if  {Jpara  >  1}  then  { 

echo  Attention:  trying  to  set  an  arm.pose.parameter  >  1 
set  arm.pose.parameter  1 

>  else  { 

set  arm.pose.parameter  Jpara 

> 

> 

> 

> 

return  Jarm.pose.parameter 


#####*#################»»#####*###»##***#*#**»*#**»«**»**************** 

t  bendjL-flngerjoint 

proc  bend.a.finger. joint  {  {finger  index}  {joint  1}  {option  by}  {angle  10}  \ 

{timesteps  2}  {renopt  ren}  }  { 

if  {$joint  >  2}  then  { 

echo  Please  specify  either  joint  1  or  2. 
return 

} 

if  {$f inger  ==  "thumb"}  then  { 

bend.thurab.joint  $joint  $option  Jangle  Jtimesteps  Jrenopt 
}  else  { 

set  limb  /l_arm/Jf inger 
set  joint.nr  [plus  Jjoint  1] 

move.a.joint  Jlimb  Jjoint.nr  Joption  Jangle  Jtimesteps  range_anglel8  noren 
if  {Jjoint  ==  2}  then  { 
incr  joint.nr 

set  angle  [mult  0.6667  Jangle] 

move.a.joint  Jlimb  Jjoint.nr  Joption  Jangle  Jtimesteps  range_anglel8  noren 

} 

if  {Jrenopt  ==  “ren"}  ren 

} 

} 


*  bendjelbow 

##############*###########*#*############*##**#########*####*# 

proc  bend.elbow  {  {option  to}  {angle  10}  {timesteps  2}  {renopt  ren}  }  { 

set  limb  /l.arra 
set  joint.nr  10 

set  min.angle  [get.min.angle  elbow] 

set  max.angle  [get.max.angle  elbow] 

set  restpos  10 

set  off  90 

set  direc  1 

t 

#  Hake  sure  given  angle  is  inside  allowed  range 
t  If  it  is  not,  find  an  appropriate  angle 

t 

set  new. angle  [in.range  Jlimb  Jjoint.nr  Joption  Jangle  Jmin.angle  \ 
Jmax.angle  Jrestpos  Joff  Jdirec  Elbow  bent] 


if  {(Joption  ==  "by")  ft  (Jnew.angle  ==  0)}  { 
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•  Tha  joint  sas  already  at  it’s  limit 
echo  already  at  limit 
return  limit 

> 

« 

#  Transform  angle  into  dhangle  range  (  90  -  240  ) ,  Restpos  100 

* 

«  set  new.angle  [mult  Jnes.angle  Jdirec] 

if  {($option  «==  "to")  I  (Joption  ==  "rest")}  { 
set  dhangle  [plus  Jneo. angle  $off] 

}  else  { 

set  dhangle  Jnew.angle 

> 

move_a_joint  $limb  Jjoint.nr  Joption  Jdhangle  Jtimesteps  range_angle36  $renopt 

m2ele  5  [get.elbow_angle]  $limb/angle . cur 

if  {Jne«r_angle  !=  Jangle}  { 

t  The  joint  has  just  reached  it’s  limit 
echo  limit  just  reached 
return  limit 

> 

return 

> 


############################*##*#«######*#############*################ 

*  bend_thumb-joint 

########«#######»######*####*############*###»*»######»####*####*##**** 

proc  bend_thumb_joint  {  {joint  2}  {option  by}  {angle  10}  {timesteps  2}  {renopt  ren}  }  { 

set  limb  /l_arm/thumb 
set  joint.nr  [plus  Jjoint  4] 

if  {(Jjoint.nr  ==  S)  t  (Joption  ==  "to")}  { 
set  off  -90 

set  angle  [plus  Jangle  Joff] 

} 

move_a_joint  Jlimb  Jjoint.nr  Joption  Jangle  Jtimesteps  ranga_anglel8  Jrenopt 

} 


*  calc_angle_timesteps 

####«############*#########################»###############*########### 

proc  calc_angle_timesteps  {limb  axis.opt  target-axis}  { 

set  angle.diff  [vangle2  [get_J{limb}_direction  Jaxis.opt]  Jtarget.axis] 
global  angle_per.timestep 

set  timesteps  [int  [div  Jangle.diff  Jangle_per_timestep]] 
if  {[less  Jtimesteps  1]}  then  { 
set  timesteps  1 

} 

return  Jtimesteps 

} 
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tttmtttmimfnttttmtiittHtmntttittttttttttttittttttttHtmt 

#  calc.dist.timesteps 

f##########*##############*####**************************************** 

proc  calc.dist.timesteps  {goal.point  gp.opt}  { 

if  {JgP-op*  ==  "wrist"}  than  { 

set  wrist.position  [get.wrist.position] 

set  dist  [enorm  [minus  $»rist_position  $goal_point]] 

}  else  { 

if  {$gp_opt  “  “ee"}  then  { 

set  dist  [enorm  [minus  [get.ee]  $goal_point]] 

}  else  { 

echo  Wrong  gp.opt  (Jgp.opt)  in  proc  goal. reached . 
echo  Parameter  has  to  be  set  to  wrist  or  ee. 
return  999 

> 

> 

global  dist.per.timestep 

set  timesteps  [int  [dir  $dist  Jdist.per.timestep]] 
if  {[less  Jtimesteps  1]>  then  { 
set  timesteps  1 

> 

return  $timesteps 

> 


##################################*##**#*#»*#**##*»**»*#***********»»** 

t  calc.elbow.wrist.vector 

####*«###**#*#####*#######«#*#*# 

proc  calc.elbow.wrist.vector  {goal.position  gp.opt  sev}  { 

set  shoulder.goal.vector  [shoulder.goal.vector  $goal_position] 
if  {Jgp.opt  ==  "wrist"}  then  { 

set  ewv  [minus  Jshoulder.goal.vector  $sev] 

}  else  { 

if  {Jgp.opt  ==  "eo"}  then  { 

set  ewv  [minus  $shoulder_goal_vector  [plus  [wrist.ee.vector]  $sev]3 
}  else  { 

echo  Wrong  gp.opt  in  proc  calc.elbow.wrist.vector. 
echo  Parameter  has  to  be  set  to  wrist  or  ee. 
return  999 

> 

} 

return  $ewv 

} 


#############################################!################**####### 
#  calc_M7 

#######################################################««###########*** 

proc  calc_H7  {als.angle  alf.angle}  { 

* 

*  dhparameters  6  :  {theta6  0  0  -90} 

*  dhparameters  7  :  {theta7  0  0  90} 

« 

set  theta6  Sals.angle 
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sat  theta7  [minus  90  $alf_angle] 

Hset  [dhmatrix  5  /l_arm/dhc] 

Hrot  z  $theta6 
Mrot  X  -90 

Hrot  z  $theta7 
Hrot  x  90 

return  [Hgat] 

} 

#################################*##»*##**###*»#»#*******#************* 
*  calc.MlO 

###############*######*############*##*##****#*«#»*#*«##*«************ 

proc  calc_H10  {als.angle  alf_angla  at.angle  a. angle}  { 

t 

t  dhparametars  6  :  {theta6  0  0  -90} 

t  dhparametars  7  :  [theta7  0  0  90} 

t  dhparametars  8  :  [theta8  0  0  90} 

t  dhparametars  9  :  {  90  0  10  0} 

#  dhparametars  10  :  {thetalO  0  0  90} 

f 

set  theta6  $als_angle 

set  theta7  [minus  90  $aif_angle] 

set  theta8  $at_angle 

set  thetalO  [plus  $e_angle  90] 

Hset  [dhmatrix  5  /l_arm/dhc] 

Hrot  z  $theta6 
Hrot  x  -90 

Hrot  z  $theta7 
Hrot  x  90 

Hrot  z  $theta8 
Hrot  x  90 

Hrot  z  90 
Htrans  10  0  0 

Hrot  z  $thetalO 
Hrot  x  90 

return  [Hget] 

} 

jet################*#####################*#########################**** 
t  calc_shoulder_elbow_vector 

################################################*##############*»#***** 

proc  calc_shoulder_elbou_vector  {  goal.position  gp.opt}  { 

set  uhich.x  max.x 

set  sgv  [shoulder.goal.vector  $goal_position] 
set  shoulder.goal.distance  [enorm  $sgv] 
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i 

*  Calculate  the  angle  between  the  shoulder.goal.vector  and  the 
t  shoulder.elbow.vector 

* 

if  {Jgp.opt  ==  "wrist"}  then  { 

set  max_shoulder_goal_distance  18.6 

if  {$shoulder_goal_distance  >  $max_shoulder_goal. distance}  { 
echo  Goal  is  not  reachable  without  moving  the  body, 
global  reaching.problems 
set  reaching.problems  1 
return  999 

} 

set  sgv.sev.angle  [gamma  10  $shoulder_goal_distance  8.6] 
if  {$sgv_sev_angle  ==  999}  {return  999} 

}  else  { 

if  {Sgp.opt  ==  "ee"}  then  { 

set  elbow.ee.distance  [enorm  [elbow.ee.vector]] 
set  max. shoulder. goal.distance  [plus  10  $elbow_ee_distance] 
if  {$shoulder_goal_distance  >  Jmax.shoulder.goal. distance}  { 
echo  Goal  is  not  reachable  without  moving  the  body, 
global  reaching.problems 
set  reaching.problems  1 
return  999 

} 

set  sgv.sev.angle  [gamma  10  Jshoulder.goal.distance  $elboe_ee_distance] 
if  {$sgv_sev_angle  ==  999}  {return  999} 

}  else  { 

echo  Wrong  gp.opt  in  proc  calc.shoulder.elbow.vector . 
echo  Parameter  has  to  be  set  to  wrist  or  ee. 
return  999 

} 

} 

* 

t  Calculate  the  minimal  z.value  possible  for  the  shoulder.elbow.vector 

* 

set  z.range  [calc_z2_range  $sgv  10  $sgv_sev_angle] 

if  {Jz.range  ==  999}  {return  999} 

set  z.min  [lindex  $z_range  0] 

set  z.raax  [lindex  $z_range  1] 

set  sev.z  $z_min 

* 

t  Add  a  tiny  amount  to  sev.z  to  make  sure, 

#  calc.shoulder_elbow_vector.xy  finds  a  solution  for  x. 

t 

set  sev.z  [plus  $sev_z  0.0001] 

global  arm.pose.opt 
global  arm.pose.parameter 

set  new.z  [plus  $z_min  [mult  $arm_pose_parameter  [minus  $z_max  $z_min]]] 
set  sev.z  $new_z 

global  change.arm.posture.parameter 

set  change.arm.posture.parameter  $arm_pose_parameter 

set  sev  [calc.shoulder_elbow_vector.xy  $sgv  $sev_z  $sgv_sev_angle  $which_x] 
if  {$sev  ==  999}  {return  999} 
return  $sev 
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*#########################*###***#*#*****»***************************** 
t  calc_third.com  ponent 

#########################****###*********#*************#*###*,#t**#*,,# 

proc  calc.third.component  {vec.length  y  z}  { 
set  Is  [mult  Jvec.length  Jvec.length] 
set  ys  [mult  $y  $y] 
set  zs  [mult  $z  $z] 

set  T  [minus  $ls  [plus  $ys  $zs]] 
return  [sqrt  $T] 


##########################*######«#####*##############*###########*#*# 

t  calc_x2 

################################*###***#* #####################*######## 

proc  calc_x2  {vectorl  vec2_length  z2  angle}  { 

set  vecl.length  [enorm  $vectorl] 
set  xl  [lindex  Jvectorl  0] 
set  yl  [lindex  $vectorl  1] 
set  zl  [lindex  $vectorl  2] 

set  T1  [mult  [mult  $vecl_length  $vec2. length]  [cos  $angle]] 

set  T2  [mult  [mult  -2  $T1]  $xl] 

set  T3  [mult  $zl  $z2] 

set  T4  [mult  2  [mult  $T3  $xl]] 

set  T5  [mult  $yl  $yl] 

set  T6  [plus  $T5  [mult  $xl  $xl]] 

set  T7  [mult  [mult  -2  $T1]  $T3] 

set  T8  [mult  [mult  $vec2_length  $vec2_length]  $TS] 
set  T9  [mult  $T5  [mult  $z2  $z2]] 

set  p  [div  [plus  $T2  $T4]  $T6] 

set  q  [div  [plus  [minus  [plus  [plus  [mult  $T1  $T1]  $T7]  [mult  $T3  $T3]]  $T8]  $T9]  $T6] 
set  root.arg  [minus  [div  [mult  $p  $p]  4]  $q] 
if  {[less  $root_arg  0]}  { 
return  999 

> 

set  x2_min  [minus  [div  $p  -2]  [sqrt  $root_arg]] 
set  x2_max  [plus  [div  $p  -2]  [sqrt  $root_arg]] 

return  [list  $x2_min  $x2_max] 

} 


#########################################################*############# 

*  calc_z2_range 

############»###############»#############################*#*########## 

proc  calc_z2_range  {vectorl  vec2_length  angle}  { 

set  xl  [lindex  $vectorl  0] 
set  yl  [lindex  $vectorl  1] 
set  zl  [lindex  $vectorl  2] 
set  vecl.length  [enorm  $vectorl] 


set  noglob  1 
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set  terml  [expr  {  Jvec2_length«Jvec2_length  /  (Jvecl_length*Jvecl_length)}] 
set  p  texpr  {  -2  *  $zi  *  Jvec2_length  /  $vecl_length  *  [cos  Jangle] >] 
set  q  [expr  {  -1  *  Jtermi  *  (Jyl*Jyl  +  $xl*Jxl  -  \ 

(Jvecl_length*Jvecl_length)  *  [cos  $angle]*[cos  Jangle])}] 
set  root.arg  [expr  {  Jp*Jp  /  4  -  Jq  }] 

if  {[less  Jroot.arg  0]}  { 
return  999 

} 

set  z2_min  [minus  [div  Jp  -2]  [sqrt  Jroot.arg]] 
set  z2_max  [plus  [div  Jp  -2]  [sqrt  Jroot.arg]] 
return  [list  Jz2_min  Jz2_raax] 


##############################«########»####################*##«###*# 

t  direct.hand.palm.towards 

ft##############*#############*#########**##*#****#******************** 

proc  direct.hand.palm.towards  {object  {time_opt  0}  {timesteps  10} 

{end.event  {}}  }  { 

* 

t  check  uhether  the  initial  conditions  are  fulfilled 
t  and  put  the  command  on  the  list 

* 

set  conditionOa  1 

if  {  JconditionOa}  then  { 

global  direct_hand_palm_toaards_end_event 

set  direct_hand_palm_towards_end_event  $end_event 

global  ic 

set  end.time  [plus  Jic  Jtimesteps] 

add_c  [list  dirct_hand_palm_tosards_statel  Jobject  Jtime.opt  Jend.time] 

}  else  { 

echo  Sorry,  the  initial  conditions  for  executing  this  skill  are  not  given! 

} 

Com_Update_cb 

return 


################»###############*«##########*## #########*######»«###### 
t  direct_hand_palm_towards_statel 

############«########################*#####«##########*################ 

proc  direct_hand_palm_touards_statel  {object  time_opt  end.time}  { 

set  point  [centroid  Jobject] 

set  ee  [get.ee] 

set  palm.axis  [minus  Jpoint  Jee] 

* 

#  State  ending  condition 

t 

set  direct.hand.palm.toaards.conditionl  [test.hand.direction  palm  Jpalm.axis] 

if  Jdirect.hand.palm.touards.conditionl  then  { 
global  direct.hand.palm.toaards.end.event 
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eval  $direct_hand_palm_toBards_end_event 

return  stop 

> 

if  {$time_opt  ==  0>  then  {  .  . 

set  timesteps  [calc.angle.timesteps  hand  palm  Jpalm.axisJ 

>  else  { 

global  ic  . 

set  timesteps  [minus  $end_time  $icj 
if  {[lesseq  Jtimesteps  0]>  then  { 
set  timesteps  1 

} 

> 

set  hand.angles  [hand.angles.rel  Jpalm.axis  no  Jtimasteps] 
if  [equal  $hand_angles  999]  then  { 

echo  Sorry,  cannot  position  the  hand.palm  axis  in  desired  direction 

return  999 

> 

set  tla_ angle  [lindex  $hand_angles  0] 
set  bh.angle  [lindex  $hand_angles  1] 
set  th.angle  [lindex  $hand_angles  2] 

if  [greater  [abs  $tla_angle]  0.0001]  then  { 
turn.lower.arm  by  $tla_angle  1  noren 

if  [greater  [abs  $bh_angle]  0.0001]  then  { 
bend.hand  by  $bh_angle  1  noren 

if  [greater  [abs  $th_ angle]  0.0001]  then  < 
toist.hand  by  $th_angle  1  noren 

> 

return 


*##*#*#####*##«*##*######*#***********mmm,**m#m,*mmm,** 

t  direct_upper-arm 

proc  direct.upper.arm  {main.axis  {perp.axis  no)  [time_opt  0)  \ 

{timesteps  10)  {end.event  {return  stop})  }  { 


set  conditionl  1 

# 

t  check  ohich  state  needs  to  be  put  on  the  commandlist 

# 

set  conditionOa  {[string  compare  $perp_axis  "no"]} 

set  conditionOb  {[equal  [string  compare  Jperp.axis  "no"]  0]} 

if  !$conditionI  then  { 

echo  Sorry,  the  initial  conditions  for  executing  this  skill  are  not  given, 
return 

) 

global  direct _upper_arm_end_event 

set  direct_upper_arm_end_event  $end_event 

global  ic 

set  end.time  [plus  $ic  $timesteps] 
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if  $conditionOa  than  { 

add.c  [list  direct _upper.arm.st at el  $main_axis  $perp_axis  $time_opt  $end_time] 

> 

if  $conditionOb  then  { 

add.c  [list  direct_upper_arm_state2  $main_axis  $time_opt  $end_time] 

> 

Com.Update.cb 

return 


9  dps 

#############################*#########*##########*#*«**##»****»*«*»** 

proc  dps  {{dist  {>}>  { 

global  dist.per.timestep 
if  {$dist  ==  "">  then  { 

echo  distance.per.tiraestep:  $dist_per_timestep 
}  else  { 

set  dist.per.timestep  $dist 

> 

return  Jdist.per.timestep 


#######*###############*###»####**##*###«###**###*#######**#«#*#****» 

t  forearm-angles_abs 

99999999999999999999999999999999999999999999999999999999999999999999999 

proc  forearm.angles.abs  {  dvec  [M7  dh7}}  { 

set  current.thetalO  [dhtheta  10  /l.arm/dhc] 
set  current_theta8  [dhtheta  8  /l.arm/dhc] 

set  dvec  [div  $dvec  [enorm  $dvec]] 
set  dO  [lindex  $dvec  0] 
set  dl  [lindex  $dvec  1] 
set  d2  [lindex  $dvec  2] 

if  {[string  compare  $M7  "dh7"]  ==  0}  then  { 
set  H7  [dhmatrix  7  /l.arm/dhc] 

> 

set  a  [lindex  $M7  0] 
set  b  [lindex  $117  4] 
set  d  [lindex  $H7  8] 
set  e  [lindex  $H7  1] 
set  f  [lindex  $M7  5] 
set  g  [lindex  $117  9] 
set  h  [lindex  $117  2] 
set  i  [lindex  $117  6] 
set  k  [lindex  $117  10] 

set  hi  [expr  {  ($dl  *  $a  -  $d0  *  $e)  *  ($b  *  $h  -  $i  »  $a)  >] 

set  h2  [expr  {  ($d2  *  $a  -  $d0  *  $h)  *  ($f  *  $a  -  $b  *  $e)  >] 

set  h3  [expr  {  ($g  »  $a  -  $d  *  $e)  *  ($b  *  $h  -  $i  *  $a)  >] 

set  h4  [expr  {  ($k  *  $a  -  $d  *  $h)  *  ($f  *  $a  -  $b  *  $e)  >] 
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if  {[abs  [plus  $h3  Jh4]]  <  0.001>  then  { 

echo  Sorry,  cannot  calculate  forearm  direction  angles  \ 

because  of  the  nature  of  matrix  of  the  upper.arm  (H7) 

set  slO  [expr  {  (Jhl  +  Jh2)  /  <Jh3  +  $h4)  >] 

9  asin  yields  “90  <=  thetalO  <s  90 
set  thetalO  [asin  JslO] 

if  {[equal  JthetalO  90] >  then  { 

set  arm_turn_angle  [get_arm.turn_angle] 

echo  arm_turn_angle  does  not  matter  ->  leave  the  current  one 
set  elbow.angle  0 

return  [list  Jarm.turn.angle  Jelbow.angle  ] 

if  {[less  JthetalO  90]  I  [greater  JthetalO  240]}  then  { 
set  thetalO  [minus  180  JthetalO] 

if  {[less  JthetalO  90]  I  [greater  $thetalO  240]}  then  { 

echo  Sorry,  the  left  lower  arm  cannot  be  positioned  in  the  \ 

specified  direction.  (Elbow  needed  to  bend  to  [minus  JthetalO  90]). 
echo  =>  Elbow  will  only  be  bent  to  ISO  degrees, 
set  thetalO  240 

} 

} 

set  clO  [cos  JthetalO] 

set  h5  [expr  {  <$dl  *  Ja  -  $d0  *  Je)  +  (Jd  *  Je  -  Jg  *  Ja)  *  JslO  }] 

set  h6  [expr  {  <$f  »  Ja  -  Jb  ♦  $e)  *  JclO  }] 

if  {[abs  $h6]  <  0.001}  then  { 

set  h5  [expr  {  (Jd2  *  Ja  -  $d0  *  $h)  +  ($d  *  Jh  -  Jk  *  $a)  *  JslO  }] 

set  h6  [expr  {  ($i  ♦  Ja  -  $b  *  $h)  *  JclO  }] 

} 

set  s8  [expr  {  JhS  /  Jh6  }] 

set  theta8a  [asin  Js8] 

set  theta8b  [minus  180  Jtheta8a] 

dhtheta  10  JthetalO  /l_arm/dhc 

t 

*  easel  (theta8a)  : 

* 

dhtheta  8  Jtheta8a  /l_arm/dhc 

set  H10  [calc_H10_from.H7_8_10  JM7  Jtheta8a  JthetalO] 
set  HlOvec  [list  [lindex  JH10  8]  [lindex  JH10  9]  [lindex  JH10  10]] 
set  diffl  [enorm  [minus  Jdvec  JHlOvec]]  ;  myecho  4  diffl  Jdiffl 
if  {Jdiffl  <  0.01}  then  { 
set  theta8  Jtheta8a 
}  else  { 

* 

t  case2  (theta8b)  : 

* 

dhtheta  8  Jtheta8b  /l_arm/dhc 

set  H10  [calc_H10.from_H7.8_10  JH7  Jtheta8b  JthetalO] 
set  HlOvec  [list  [lindex  JH10  8]  [lindex  JM10  9]  [lindex  JH10  10]] 
set  diff2  [enorm  [minus  Jdvec  JHlOvec]]  ;  myecho  4  diff2  Jdiff2 
if  {Jdiff2  <  0.01}  then  { 
set  theta8  Jtheta8b 
}  else  { 

« 

t  case3  (theta8)  : 

9 

9  In  case  thetalO  was  artificially  set  to  240  (cause  it  cannot 
9  reach  any  bigger  angle)  then  no  theta8  will  satisfy  the 
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♦  equations  exactly.  Take  the  better  one  of  the  theta8’s. 
if  {[less  $diffi  $diff2]>  then  { 
set  theta8  $theta8a 
}  else  { 

set  theta8  $theta8b 

> 

> 

> 

dhtheta  8  $current_theta8  /l_arm/dhc 
dhtheta  10  $current_thetalO  /l_arm/dhc 

set  arm_turn_angle  $theta8 

set  elboB.angle  [minus  JthetalO  90] 

return  [list  $arm_turn_angle  $elboB_angle  ] 

> 


99999999999999999999999999999999999999999999999999999999999999999999999 

9  gamma 

99999999999999999999999999999999999999999999999999999999999999999999999 

proc  gamma  {  a  b  c  }  { 

set  argument  [div  [minus  [plus  [mult  $a  $a]  [mult  $b  $b]]  \ 

[mult  $c  $c]]  [mult  [mult  2.0  $a]  $b]  ] 
if  [greater  [abs  $argument]  1]  then  { 

echo  The  absolut  argument  for  acos  in  proc  gamma  is  greater  than  1 . 
return  999 

> 

set  gamma  [acos  $argument] 
return  $gamma 

> 


*#######################################*############################## 

9  get_elbow_angle 

*#######* 

proc  get_elbow_angle  ■(}  { 

set  limb  /l_arm 

set  dhc  $limb/dhc 

set  joint.nr  10 

set  off  90 
set  direc  1 

set  angle  [mult  [minus  [dhtheta  $joint_nr  $dhc]  $off]  $direc] 
return  $angle 

> 


###############################«########«####*########################* 

t  get.max-angle 

99999999999999999999999999999999999999999999999999999999999999999999999 
proc  get.max.angle  {  jointmotion  >  { 


case  $jointmotion  in  { 
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shoulder.fb  {return  40} 
shoulder.ld  {return  4S> 
arin_ls  {return  180} 

arm.lf  {return  190> 

arm.turn  {return  175> 
elbow  {return  150} 

hand.turn  {return  190} 
hand.bend  {return  80} 
hand.twist  {return  35} 

} 

echo  Invalid  parameter  option  in  get.max.angle 

return 

} 

##########################*****»***********#****i****##****#*#*,**t*##, 

«  get.min^angle 

proc  get_min_angle  {  jointmotion  }  { 

case  Sjointmotion  in  { 

shoulder.fb  {return  -20} 
shoulder.ld  {return  -5} 
arm.ls  {return  -180} 

arm.lf  {return  -90} 

arm.turn  {return  -170} 
elbow  {return  0} 

hand.turn  {return  -40} 
hand.bend  {return  -80} 
hand.twist  {return  -35} 

} 

echo  Invalid  parameter  option  in  get.max.angle 

return 

} 

*  get_shoulder_position 

proc  get.shoulder.position  {}  { 
set  m  [dhmatrix  5  /l.arm/dhc] 

return  [list  [lindex  $m  12]  [lindex  $m  13]  [lindex  $m  14]] 

} 

#################*##*################*########*############***#****»*»* 

#  goal.arm-angles.abs 

###############################################*##########*##*##*##*#** 

proc  goal.arm.angles.abs  {goal.position  gp.opt}  { 

* 

t  Calculate  the  jointangles  for  the  arm.ls,  arm.lf,  arm.turn,  and 
#  elbow  joint 

t 

set  sev  [calc.shoulder.elbow.vector  $goal_position  Jgp.opt] 
if  {$sev  ==  999}  {return  999} 
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set  upper.arm.angles  [upper_arm_angles_abs  $sev  no] 
if  {$upper_arm_angles  ==  999)  {return  999} 
set  neu_arm_ls  [lindex  $upper_arm_angles  0] 
set  neu.arm.lf  [lindex  $upper_arm_angles  1] 

* 

t  Calculate  the  nea  7.  dhmatrix  given  the  new  upper.arm.angles 

* 

set  H7  [calc_H7  $neo_arm_ls  $new_arm_lf] 
if  {$gp_opt  ==  "ee")  then  { 

set  forearm_angles  [calc_at_e  $M7  $goal. position] 
if  {$forearm_angles  ==  999}  {return  999} 

}  else  { 

set  ewv  [calc_elbow_wrist_vector  $goal_position  $gp_opt  $sev] 
set  f orearm_angles  [forearm_angles„abs  $ewv  $M7] 
if  {$forearm_angles  ==  999}  {return  999} 

} 

return  [list  $neu_arm_ls  $new_arm_lf  [lindex  $forearm_angles  0]  [lindex  $f orearm_angle3  1]] 


########*########*#####################################*#######*####**# 
«  hand-angles_rel 

#######*#######*#########*»*#»####*«#*#####*#*###«*#*«*»***********»»»* 

proc  hand_angles_rel  {  palm.axis  point.axis  timesteps}  { 
set  current_hand_turn_angle  [get_hand_turn_angle] 
set  current_hand_bend_angle  [get_hand_bend_angle] 
set  current_hand_tuist_angle  [get_hand_tuist_angle] 

if  {[llength  $point_axis]  ==  3}  then  { 

set  point.axis  [div  $point_axis  [enorm  $point_axis]] 
set  mO  [lindex  $point_axis  0] 
set  ml  [lindex  $point_axis  1] 
set  m2  [lindex  $point_axis  2] 

} 

set  palm.axis  [div  $palm_axis  [enorm  $palm_axis]] 
set  nO  [lindex  $palm_axis  0] 
set  nl  [lindex  $palm_axis  1] 
set  n2  [lindex  $palm_axis  2] 

set  H10  [dhmatrix  10  /l_arm/dhc] 

set  a  [lindex  $1110  0] 

set  b  [lindex  $M10  4] 

set  d  [lindex  $1110  8] 

set  e  [lindex  $N10  1] 

set  f  [lindex  $M10  5] 

set  g  [lindex  $1110  9] 

set  h  [lindex  $1110  2] 

set  i  [lindex  $N10  6] 

set  k  [lindex  $1110  10] 

*««##»»#»»$»  Calculation  of  thetal3  tttttttttttt 

set  hi  [expr  {  ($e  *  $n0  -  $a  *  $nl)  *  ($b  *  $h  -  $i  *  $a)  }] 

set  h2  [expr  {  ($h  *  $n0  -  $a  *  $n2)  *  ($f  *  $a  -  $b  *  $e>  }] 

set  h3  [expr  {  ($g  *  $a  -  $d  *  $e)  *  ($b  *  $h  -  $i  *  $a)  }] 

set  h4  [expr  {  ($k  *  $a  -  $d  *  $h)  *  ($f  »  $a  -  $b  *  $e)  }] 


if  {[abs  [plus  $h3  $h4]]  <  0.001}  then  { 
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echo  Sorry,  cannot  calculate  hand  direction  angles  \ 

because  of  the  nature  of  matrix  of  the  hand  (M14) 

set  sl3  [expr  {  <$hl  +  $h2)  /  ($h3  +  $h4>  >] 
set  thetal3  [asin  $sl3] 
f  asin  yields  -90  <=  thetal3  <=  90 

* 

*  Check,  whether  thetai3  is  outside  of  it’s  allowed  range. 

f  If  this  is  the  case,  set  it  to  it’s  maximum/  minimum  angle. 

* 

if  {[less  $thetai3  -80]  I  [greater  $thetal3  80]}  then  { 

echo  Sorry,  the  left  hand  cannot  be  positioned  pointing  in  the 
echo  specified  direction.  (Hand  needed  to  bend  to  $thetal3). 

9  or  [minus  180  $thetal3] 

echo  Therefore  the  hand  will  be  bent  as  far  as  possible,  to 
if  {[less  $thetal3  -80]}  then  { 
echo  -80  degrees, 
set  thetal3  -80 
}  else  { 

echo  80  degrees, 
set  thetal3  80 

} 

} 

dhtheta  13  $thetal3  /l_arm/dhc 

############  Calculation  of  thetall  99999999999 9 
set  cl3  [cos  $thetal3] 

set  hS  [expr  {  ($e  *  $n0  -  $a  *  $nl)  +  <$d  *  $e  -  $g  *  $a)  *  $sl3  }] 

set  h6  [expr  {  ($f  *  Ja  -  $b  *  $e)  *  $cl3  }] 

if  {[abs  $h6]  <  0.001}  then  { 

set  h5  [expr  {  ($h  *  $n0  -  $a  *  $n2)  -  <$d  *  $h  -  $k  *  $a)  *  $sl3  }] 

set  h6  [expr  {  ($i  *  $a  -  $b  *  $h)  *  $cl3  }] 

} 

set  ell  [expr  {  $h5  /  $h6  }] 

t  acos  yields  in  0  <=  thetall  <=  180 
set  thetalla  [acos  Sell] 

#  180  <=  thetallb  <=  360 

set  thetallb  [minus  360  Sthetalla] 

* 

*  Find  out,  which  one  of  the  two  solutions  for  thetall  is  the  right  one 

* 

•  Hote  that  the  palm  vector  of  the  hand  is  not  influenced 
9  by  thetal4  (hand  twisting  angle) ,  so  that  it  does  not 
t  matter  for  the  following  test,  what  thetal4  is. 

9 

9  case  1  (thetall)  : 

dhtheta  11  $thetalla  /l_arm/dhc 

set  angle_diffl  [abs  [vangle2  [get_hand_direction  palm]  $palm_axis]] 
if  [less  $angle_diffl  0.1]  then  { 
set  thetall  $thetalla 
}  else  { 

9  case  2  (thetall)  : 

dhtheta  11  $thetallb  /l_arm/dhc 

set  angle_diff2  [abs  [vangle2  [get_hand_direction  palm]  Jpalm.axis]] 
if  [less  angle_diff2  0.1]  then  { 
set  thetall  Jthetallb 
}  else  { 

# 
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9  case  3  (thetall)  : 

9  Take  the  best  possible  solution. 

9 

if  {[less  Jangle.diffl  $angle_diff2]>  then  { 
set  thetall  $thetalla 
}  else  { 

set  thetall  $thetallb 

> 

> 

> 

9 

9  How ,  check  whether  thetall  is  inside  it’s  allowed  range 
9  If  not,  set  it  to  it’s  maximal  or  minimal  allowed  angle. 

9 

9  set  thetall.min  [plus  [get_min_angle  hand.turn]  90]  =>  50 
9  set  thetali.max  [plus  [get_max_angle  hand.turn]  90]  =>  280 

9 

if  [less  Sthetall  50]  then  { 

echo  Sorry,  the  left  hand  cannot  be  positioned  pointing  in  the  specified  direction, 
echo  The  hand  needed  to  be  turned  to  [minus  $thetall  90]  \ 

but  can  only  be  turned  to  -40  degrees  for  physical  reasons, 
set  thetall  50 

> 

if  [greater  $thetall  280]  then  { 

echo  Sorry,  the  left  hand  cannot  be  positioned  pointing  in  the  specified  direction, 
echo  The  hand  needed  to  be  turned  to  [minus  {thetall  90]  \ 

but  can  only  be  turned  to  190  degrees  for  physical  reasons, 
set  thetall  280 

> 

dhtheta  11  {thetall  /l.arm/dhc 

999999999999  Calculation  of  thetal4  999999999999 

if  {[llength  {point.axis]  ==  3}  then  { 

set  hi  [expr  {  <{a  *  {m2  -  $h  *  $m0)  *  ($f  »  $a  -  $b  *  $e)  >] 

set  h2  [expr  {  ($a  *  {ml  -  $e  *  $raO)  *  <$b  *  $h  -  $i  *  $a)  >] 

set  h3  [expr  {  <$g  *  {a  -  {d  *  $e)  •  ($b  *  $h  -  $i  *  $a)  >] 

set  h4  [expr  {  ($k  *  {a  -  {d  *  $h)  *  ($f  *  $a  -  $b  *  $e)  >] 

9  Hote:  thetal3  can  never  be  90  or  -90,  so  cl3  will  never  be  0 

if  {[abs  [plus  $h3  $h4]]  <  0.001}  then  { 

echo  Sorry,  cannot  calculate  hand  direction  angles  \ 
because  of  the  nature  of  matrix  of  the  hand  (H14) 

> 

set  cl4  [expr  {  ({hi  +  $h2)  /  (<$h3  +  $h4)  *  $cl3)  }] 

set  thetal4  [acos  $cl4] 

set  thetal4b  [mult  -1  $thetal4] 

9  case  1  (thetal4)  : 
dhtheta  14  $thetal4  /l_arm/dhc 

set  angle.diffl  [abs  [vangle2  [get.hand.direction  point]  {point.axis]] 
if  [less  $angle_diffl  0.1]  then  { 

}  else  { 

9  case  2  (thetal4)  : 
dhtheta  14  $thetal4b  /l_arm/dhc 

set  angle_diff2  [abs  [vangle2  [get.hand.direction  point]  {point.axis]] 
if  [less  $angle_diff2  0.1]  then  { 
set  thetal4  $thetal4b 
}  else  { 
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* 

#  case  3  (thetal4)  : 
t  Take  the  best  possible  solution 

f 

if  [greater  Jangle.diffl  Jangle_diff2]  then  { 
set  thetal4  $thetal4b 

> 

> 

> 

* 

*  Check,  whether  thetal4  is  inside  it’s  allowed  range. 

*  If  it  is  not,  set  it  to  it’s  allowed  maximum/  minumum. 

* 

if  {[less  $thetal4  -35]  I  [greater  Jthetai4  35]}  then  { 
echo  {} 

echo  Sorry,  the  left  hand  cannot  be  positioned  pointing  along  \ 
the  specified  point  axis. 

echo  (Hand  needed  to  be  twisted  to  [mult  -1  $thetal4]) 
echo  But  I  will  do  the  best  I  can  ! 

echo  Palm.axis  might  be  as  specified,  but  pointing. axis  will  not. 
echo  You  could  try  to  change  my  arm.turn  angle  or  \ 

elbow.angle  to  reach  the  point.axis  you  specified! 
echo  (You  have  to  find  the  direction  of  the  change  though.) 
echo  {} 

if  {[less  $thetal4  -35]}  then  { 

echo  Hand  is  supposed  to  twist  [plus  $thetal4  35]  degrees  more,  but  it  cannot! 
set  thetal4  -35 
}  else  { 

t  (meaning,  if  [greater  $thetal4  35]) 

echo  Hand  is  supposed  to  twist  [minus  $thetal4  35]  degrees  more,  but  it  cannot! 
set  thetal4  35 

} 

} 

}  else  { 

set  thetal4  0 


«  Set  the  joints  back  to  the  original  angles 
turn.lower.arm  to  Jcurrent.hand.turn.angle  1  noren 
bend.hand  to  Jcurrent.hand.bend.angle  1  noren 

twist.hand  to  Jcurrent.hand.twist.angle  1  noren 

set  tla.angle.diff  [minus  [minus  Jthetall  90]  Jcurrent.hand.turn.angle] 

set  bh.angle.diff  [minus  Jthetal3  Jcurrent.hand.bend.angle] 

set  th.angle.diff  [minus  [mult  -1  Jthetal4]  Jcurrent.hand.twist.angle] 

t 

t  angle  step  vector  =  angle  difference  vector  /  new.times 

* 

set  tla.angle.step  [div  Jtla.angle.diff  Jtimesteps] 
set  bh.angle.step  [div  Jbh.angle.diff  Jtimesteps] 
set  th.angle.step  [div  Jth.angle.diff  Jtimesteps] 

return  [list  Jtla.angle.step  Jbh.angle.step  Jth.angle.step] 
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#  in-range 

proc  in.range  {  limb  joint.nr  option  angle  min.angle  max_angle  restpos  off  direc  part  action  }  { 

« 

*  Hake  sure  given  angle  is  inside  allowed  range 

* 

case  $option  in  { 
to  { 

if  {[less  $angle  $min_angle]>  then  { 
echo  {} 

echo  Given  angle  is  outside  of  the  allosed  range, 
echo  Therefore,  the  Jpart  Bill  be  $action  to  the  minimum 
echo  position  of  $min_angle  degrees, 
set  angle  $min_angle 

>  else  { 

if  [greater  $angle  Jmax_angle]  { 
echo  {} 

echo  Given  angle  is  outside  of  the  allowed  range, 
echo  Therefore,  the  Jpart  sill  be  Jaction  to  the  maximum 
echo  position  of  Jmax.angle  degrees, 
set  angle  Jmax_angle 

> 

> 

> 

by  { 

set  dhc  Jlimb/dhc 

set  current-angle  [mult  [minus  [dhtheta  Jjoint_nr  Jdhc]  Joff]  Jdirec  ] 
set  test-pos-angle  [plus  Jangle  Jcurrent.angle] 
if  [less  Jtest-pos_angle  Jmin_angle]  then  { 

set  angle  [minus  Jmin_angle  Jcurrent_angle] 

echo  Jpart  would  be  Jaction  to  an  angle  outside  of  the  allowed  range, 
echo  Therefore,  the  Jpart  will  be  Jaction  by  maximal  Jangle  degrees. 

}  else  { 

if  [greater  Jtest_pos_angle  Jmax_angle]  { 

set  angle  [minus  Jmax.angle  Jcurrent.angle] 

echo  The  Jpart  would  be  Jaction  to  an  angle  outside  of  the  allowed 
echo  range.  Therefore  Jpart  will  be  Jaction  by  maximal  Jangle  degrees. 

> 

> 

> 

rest  {  set  angle  Jrestpos  } 

> 

return  Jangle 

> 

#########«###############»###»########»  I#################*############ 
t  insert.c 

####################»###########################«#####*#*####!######### 

proc  insert_c_list  {list  nr_next_command}  { 
global  commandlist ; 
foreach  comm  Jlist  { 

set  commandlist  [linsert  Jcommandlist  Jnr.next.command  Jcomm] 
incr  nr_next_command 

> 

return 

> 
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##############**##**************#***#*#*#m**#*m*m#**##**##m**## 
#  left-circle 

#################***#*******#****#***,#*#m*****mmm#*,,##m**#* 


proc  left-circle  {radius}  { 


y 


echo  rl 
echo  {} 
echo  11 
echo  r2 
echo  {} 
echo  12 
echo  r3 
echo  {} 
echo  13 
echo  r4 
echo  {} 
echo  14 
echo  r5 
echo  {} 
return 


»  [set  rl  0] 

=  [set  11  [mult  $radius  [minus  1  [cos  22.5]]]] 

=  [set  r2  [mult  $radius  [sin  22.5]]] 

=  [set  12  [minus  [mult  $radius  [minus  1  [cos  45]]]  $11]] 

»  [set  r3  [mult  Jradius  [sin  45]]] 

=  [set  13  [minus  [minus  [mult  $radius  [minus  1  [cos  67.5]]]  $12]  $11]  ] 
=  [set  r4  [mult  $radius  [sin  67.5]]] 

=  [set  14  [minus  [minus  [minus  $radius  $13]  $12]  $11]] 

=  [set  r5  $radius] 

[list  $rl  $11  $r2  $12  $r3  $13  $r4  $14  $rS  ] 


t  make-bone 

############«###»#**#************************************************* 


proc  make-bone  {name  left.radius  right.radius  length  {precision  15}}  { 


set  one  [left-circle  $left_radius] 
set  two  [right-circle  $right_radius] 


} 


make-bone-data  $name  $precision  10  \ 

[lindex  lone  0]  [lindex 
[lindex  $one  3]  [lindex 
[lindex  $one  6]  [lindex 
$length  [lindex  $two  0]  [lindex 
[lindex  $two  3]  [lindex 
[lindex  $two  6]  [lindex 


lone 

$one 

lone 

$tso 

$two 

$tso 


1] 

4] 

7] 

1] 

4] 

7] 


[lindex  $one  2]  \ 
[lindex  $one  5]  \ 
[lindex  lone  8]  \ 
[lindex  $tso  2]  \ 
[lindex  $two  5]  \ 
[lindex  $two  8] 


#####################################################*#############*### 
«  make-bone-data 

############################»####################*»####*####*######*»*# 


proc  make-bone-data  {name  precision  nrrad  rl  11 
{12  1}  {r3  1}  {13  1}  {r4  1} 
{r6  1}  {16  1}  {r7  1}  {17  1} 
{19  1}  {rlO  1}  {110  1}  {rll 
{rl3  1}  {113  1}  {rl4  1}  }  { 


r2  \ 

{14  1}  {r5  1}  {15  1}  \ 

{r8  1}  {18  1}  {r9  1}  \ 

1}  {111  1}  {rl2  1}  {112  1}  \ 


if  {$name  ==  ""}  then  { 

set  filename  ‘/graphics/tcl/data/cylinder .asc 
}  else  { 

set  filename  "/graphics/tcl/data/$name .asc 

} 

set  file 
set  nopts 


[open  $filename  u] 

[mult  $precision  $nrrad] 
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set  nrmantelpatches  [mult  Jprecision  [minus  Jnrrad  1]] 
set  nopolys  [plus  Jnrmantelpatches  2] 

puts  Jfile  "Jnopts  Jnopolys" 

set  step  [expr  360.0/$precision] 
set  limit  [expr  360.0-$step/2] 
set  count  0 

set  nrlength  [minus  Jnrrad  1] 
set  length  0 

for  {set  nrl  1}  {Jnrl  <=  Jnrlength}  {incr  nrl}  { 
set  length  [plus  $length  [set  ljnrl]] 

} 

set  x  [mult  $length  -i] 

for  {set  nr  1}  {$nr  <=  $nrrad}  {incr  nr}  { 

for  {set  angle  0.0}  {Jangle<Jlimit}  {set  angle  [plus  Jangle  $step]}  { 
incr  count 

set  z  [mult  [set  r$nr]  [cos  Jangle]] 
set  y  [mult  [set  rjnr]  [sin  Jangle]] 

if  {[abs  Jz]  <  0.0001}  {set  z  0.0} 
if  {[abs  Jy]  <  0.0001}  {set  y  0.0} 
puts  Jfile  "Jx  Jy  Jz" 

} 

set  x  [plus  Jx  [set  ljnr]] 
if  {[abs  Jx]  <  0.0001}  {set  x  0.0} 

} 

for  {set  k  1}  {Jk  <  Jnrmantelpatches}  {set  k  [plus  Jk  Jprecision]}  { 
for  {set  i  Jk}  {Ji  <=  [plus  Jk  [minus  Jprecision  2]]}  {incr  i}  { 
set  next  [plus  Ji  Jprecision] 

puts  Jfile  "4  Ji  [plus  Ji  1]  [plus  Jnext  1]  Jnext" 

} 

puts  Jfile  "4  Ji  Jk  [plus  Jk  Jprecision]  [plus  Ji  Jprecision]" 

} 

set  all  {} 

for  {set  n  Jprecision}  {Jn  >=  1}  {set  n  [minus  Jn  1]}  { 
append  all  "  Jn" 

} 

puts  Jfile  "Jprecision  Jail" 
set  all  {} 

for  {set  n  [plus  [minus  Jnopts  Jprecision]  1]}  {Jn  <=  Jnopts}  {incr  n}  { 
append  all  "  Jn" 

} 

puts  Jfile  "Jprecision  Jail" 
close  Jfile 

return  "OSU  file  Jfilename  successfully  written" 


########################»##########################*######!*#»#»#»**### 
*  move_a  joint 

###«###########*###############«######################################* 

proc  move.a.joint  {limb  joint.nr  option  dhangle  {timesteps  2}  \ 

{range.angle  range_angle!8}  {renopt  ren}}  { 


set  dhc 


Jlimb/dhc 
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* 

«  Find  out  if  limb  and  joint  exist 

# 

if  {(Joption  ==  "to")  I  ($option  ==  "rest")}  then  { 
set  dhtheta.current  [dhtheta  Jjoint.nr  $dhc] 

* 

#  Find  out  nhich  angle  the  joint  has  to  move  to  reach 

#  the  specified  angle  (joint  position) 

# 

set  dhangle  [minus  $dhangle  $dhtheta_current] 

>  else  { 

if  [Soption  !=  "by"}  { 

echo  Wrong  option:  You  can  either  choose 
echo  "by"  =>  bend  joint  by  specified  angle  or 
echo  "to"  =>  bend  joint  to  specified  angle 
return 

} 

> 

if  [less  [abs  Jdhangle]  0.1]  then  { 
set  timestep3  1 

> 

set  step  [div  Jdhangle  $timesteps] 

for  [set  i  1}  [$i  <=  $timesteps>  [incr  i}  { 

set  dhtheta_current  [dhtheta  $joint_nr  $dhc] 

* 

<  Range  the  angles  in  case  they  get  out  of  the  range  0-360 

* 

set  dhtheta.new  [$range_angle  [plus  $dhtheta_current  $step]  ] 
dhtheta  $joint_nr  $dhtheta_new  $dhc 
if  {$renopt  ==  "ren“>  ren 

} 

return 

> 

•  move^arm.to.goal 

t #######################*###############»**#***»*###*##**************** 

proc  move.arm.to.goal  [goal.point  [gp.opt  ee}  {time. opt  0}  [timesteps  1}  [end.event  {}}}  { 

* 

t  check  uhether  the  initial  conditions  are  fulfilled 
t  and  put  the  command  on  the  list 

* 

*  set  conditionOa  [goal_in_reaching_distance] 
set  conditionOa  1 

if  {  JconditionOa}  then  { 

global  move_arm_to_goal_end_event 

set  move_arm_to_goal_end_event  $end_event 

global  ic 

set  end.time  [plus  $ic  Jtimesteps] 

add_c  [list  move_arm_to_goal_statel  $goal_point  $gp_opt  $time_opt  $end_time] 

}  else  { 

echo  Sorry,  the  initial  conditions  for  executing  this  skill  are  not  given! 

} 

Com.Update.cb 

return 


} 


Source  Code  Examples 


133 


*##################*#*#######*######*##*###########################»*## 
*  object.l.axis 

I#####*###**############*####*### #*##»####*##»#############*########### 

proc  object_l_axis  {object}  { 
set  m  [matrix  ^object] 

return  [list  [lindex  $m  8]  [lindex  $ra  9]  [lindex  $m  10]  ] 

} 


*t#f  ##############*####»##*##«*###*###############*###*#*##*****##**#» 

*  put_arm_on_table 

###############**#######*****###*#########«###**#*#*»*»*»«»####*##*##» 

proc  put_arm_on_table  {  {end.event  {}}  }  { 

# 

t  Check  whether  the  initial  conditions  are  fulfilled 
S  and  put  the  command  on  the  list 

• 

set  conditionO  [joint_in_box  wrist  table  z_plus_in] 

if  $conditionO  then  { 

global  put_arm_on_table_end_event 

set  put_arm_on_table_end_event  $end_event 

add.c  {put_arm_on_table_statel} 

Com_Update_cb 
}  else  { 

echo  Sorry,  the  initial  conditions  for  executing  this  skill  are  not  given! 

} 

return 

> 


#####»####################*####»##«###########»# 
f  put_arm_on_table^statel 

proc  put_arm_on_table_statel  {}  { 

t 

t  State  ending  condition 

« 

set  conditionla  [collision_lower_arm  /table] 

set  conditionlb  [collision.f inger  /table] 

if  Sconditionla  then  { 

add.c  {put_arm_on_table.state2> 
return  stop 

} 

if  Jconditionlb  then  { 

add.c  {put_arra_on_table_state3> 
return  stop 

> 

bend.elbow  by  -1  1  noren 

> 
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#############**#####*#****#***##******##*********#**********#********** 
t  put_arm_on_table_state2 

proc  put.arm_on_table.stat  e2  -Q  { 

* 

t  State  ending  condition 

S 

set  condition2  [collision.finger  /table] 

if  {$condition2}  then  { 

global  put_arm_on_table_end_event 
eval  $put_arm_on_table_end_event 
return  stop 

> 

bend_hand  by  1  1  noren 

> 

##############t####*#*##»*****#***#*********#*#**####***M*#m###*,#,, 
«  put_arm_on_table-state3 

#############*#########»##*#*##*####»*********##***#*###########******# 

proc  put_arm_on_table_state3  {}  { 

« 

*  State  ending  condition 

* 

set  condition3  [equal  [collision.finger  /table]  0] 

if  <$condition3}  then  { 

add.c  {put_arm_on_table_state4} 
return  stop 

> 

bend.hand  by  -1  1  noren 

> 


*  put_arm.jon_table_state4 

proc  put_arm_on_table_state4  O  { 

t 

t  State  ending  condition 

* 

set  condition4a  [collision.lower.arm  /table] 

set  condition4b  [collision.finger  /table] 

if  -{$condition4a}  then  { 

add.c  {put_arm_on_table_state2} 
return  stop 

> 

if  [$condition4b>  then  { 

add.c  [put_arm_on_table.state3> 
return  stop 

> 

bend.elbos  by  -1  1  noren 

bend.hand  by  -1  1  noren 

> 
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#####################################*###*#*»**#***#*********»*»»»***** 

f  reacli_angles_abs 

proc  reach_angles_abs  {goal.point  gp_opt  thumb_axis  hand_bend_angle}  { 

sat  arm.angles  [goal_arm_angles_abs  $goal_point  $gp_opt] 
if  {$arm_angles  »  999>  {return  999) 

sat  H10  [calc.MlO  [lindex  $arra_angles  0]  [lindex  $arm_angles  1]  \ 

[lindex  $arm_angles  2]  [lindex  $arra_angles  3]] 
sat  hand.angles  [aligned_hand_angles_abs  $thumb_axis  $hand_bend_angle  $H10  ] 
if  {Jhand.angles  ==  999}  {return  999} 

return  [concat  [get_shoulder_fb_angle]  [gat_shoulder_ld_angle]  Sarm.angles  $hand_angles] 

} 

mmmstmttmiitiitKtiismtmsiittmiimmmmitittmKi 

<  rem_c 

##*###############*################*#############*#»###**#*##**##**#*** 

proc  rem_c  {  command  }  { 
global  commandlist; 

set  nr  [lsearch  $commandlist  $command] 
if  {$nr  !*  -1}  {rem.nth  $nr} 

} 

##################«###«############################**####*########*#### 

*  rcrn_nth 

#########*########*#######################################*###########* 

proc  rem_nth  {n}  { 

global  commandlist 
set  neslist  {} 

for  {set  i  0}  {[less  $i  [llength  $commandlist]]}  {set  i  [plus  $i  1]}  { 
if  {!  [equal  $n  $i]}  { 

lappend  nealist  [lindex  $commandlist  $i] 

} 

} 

set  commandlist  $naslist 

} 


#########«#########################*#*####################*########*### 
#  right-circle 


proc  right-circle  {radius}  { 


echo  r6  = 
echo  {} 
echo  16  = 
echo  r7  = 
echo  {} 
echo  17  = 
echo  r8  = 
echo  {} 
echo  18  = 
echo  r9  = 


[set  16  $radius] 

[set  16  [mult  Iradius  [cos  67.5]]] 

[set  r7  [mult  $radius  [sin  67.5]]] 

[set  17  [minus  [mult  Sradius  [cos  45]]  $16]] 

[set  r8  [mult  $radius  [sin  45]]] 

[set  18  [minus  [minus  [mult  $radius  [cos  22.5]]  $17]  $16]  ] 
[set  r9  [mult  $radius  [sin  22.5]]] 
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echo  {} 

echo  19  =  [set  19  [minus  [minus  [minus  $radius  $18]  $17]  $16]] 
echo  rlO  =  [set  rlO  0] 
echo  {} 

return  [list  $r6  $16  $r7  $17  $r8  $18  $r9  $19  $rlO  ] 

> 

•  server 

####################*##########*##»***#»#*********»****###************* 

proc  server  {  {nr  1000000}}  { 
global  commandlist ; 
global  loop_in  loop.abort ; 
global  ic 

if  {$loop_in}  return 
set  loop_in  1 
global  topcommands 
set  top  [Toplevel] 

if  {![nilp  $topcommands] }  {Toplevel  $topcommands} 
for  {set  i  0}  {[less  $i  $nr]}  {set  i  [plus  $i  1]}  { 
if  {$loop_abort}  break; 

for  {set  c  0}  {[llength  $commandlist]  >  $c}  {set  C  [plus  $c  1]}  { 

set  command  [lindex  $commandlist  $c] 
set  returnvalue  [eval  $command] 

if  { ($returnvalue  ==  "limit")  I  ($returnvalue  ==  "stop")  I  ($returnvalue  ==  "target")}  { 
rem_c  $command 

if  {![nilp  $topcommands]}  {Com_Update_cb} 
set  c  [minus  $c  1] 

} 

} 

execjobs 

ren 

incr  ic 

} 

set  loop.in  0 
return 

} 

9999999999999999999999999999999999999999999999999999999999999999999999* 

•  twist-liand 

ft#######################*#######################**#####*############## 

proc  tyist_hand  {  {option  to}  {angle  0}  {timesteps  2}  {renopt  ren}  }  { 

set  limb  /l.arra 
set  joint.nr  14 

set  min.angle  [get_min_angle  hand-twist] 

set  max-angle  [get_max_angle  hand-twist] 

set  restpos  0 

set  off  0 

set  direc  -1 

> 

9  Hake  sure  given  angle  is  inside  allowed  range 
9  If  it  is  not,  find  an  appropriate  angle 
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* 

set  angle  [in.range  Jlimb  $joint_nr  Joption  $angle  Jmin.angle  \ 

Jmax.angle  $restpos  $off  Jdirec  Hand  twisted] 

* 

*  Take  into  account  that  the  original  angle  might  be  off 

t  (because  of  the  formation  of  the  dhc)  and  that  the  direction  might 
9  be  reversed 

* 

set  angle  [mult  Jangle  Jdirec] 

move_a_joint  Jlimb  Jjoint.nr  Joption  Jangle  Jtimesteps  range. anglel8  Jrenopt 

m2ele  8  [get.hand.twist.angle]  Jlimb/angle .cur 
return 


99999999999999999999999999999999999999999999999999999999999999999999999 

9  upper_arm.angles_abs 

99999999999999999999999999999999999999999999999999999999999999999999999 

proc  upper.arm.angles.abs  {main.axis  perp.axis}  { 

t  It  is  important  to  make  sure  the  specified  axis  is  a  unit  vector 
set  main.axis  [div  Jmain.axis  [enorm  Jmain.axis]] 

Mset  [dhmatrix  5  /l.arm/dhc] 

Hinvert 
Hale  300 
Hole  310 
Hele  320 

set  B  [Mxforra  Jmain.axis] 

set  c7  [lindex  JB  2] 
set  theta7  [acos  Jc7] 

9  acos  yields  0  <»»  theta7  <=  180 
set  theta7b  [mult  -1  Jtheta7] 

if  {[greater  Jtheta7  160]}  then  { 

*  if  160  <  thata7 

echo  Sorry,  the  specified  direction  for  the  main  axis 
echo  of  the  upper.arra  is  not  reachable. 

echo  The  upper.arm  is  supposed  to  be  lifted  forwards  to  Jtheta7a 
echo  degrees,  but  it  will  only  be  lifted  to  160  degrees 
set  theta7  160 

set  s7  [sin  Jtheta7] 

set  c6  [div  [lindex  JB  0]  Js7] 

set  theta6  [acos  Jc6] 

t  do  the  best  you  can 

if  {[greater  [abs  [minus  [sin  Jtheta6]  [div  [lindex  JB  1]  Js7]]]  \ 
[abs  [minus  [sin  [mult  -1  Jtheta6]]  [div  [lindex  JB  1]  Js7]]]] 
}  then  { 

set  theta6  [mult  -1  Jtheta6] 

} 

}  else  { 

if  {[greater  Jtheta7  90]}  then  { 

9  if  90  <  theta7  <  160 
set  s7  [sin  Jtheta7] 
set  c6  [div  [lindex  JB  0]  Js7] 
set  theta6  [acos  Jc6] 

if  {[greater  [abs  [minus  [sin  Jtheta6]  \ 
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[div  [lindex  $B  1]  $s7]]]  0.01]  >  then  { 
sat  theta6  [mult  -1  $theta6] 

> 

}  else  { 

if  {$theta7  ==  0}  than  { 

«  thata6  doesn’t  matter  =>  leave  current  value 
set  theta6  [dhtheta  6  /l_arm/dhc] 

}  else  { 

«  if  0  <  theta7  <  90 

#  figure  out  uhether  theta7  or  theta7b  causes  the 
t  arm  to  move  the  least  joint  angle  distance  sum 

* 

set  s7  [sin  $theta7] 

set  c6  [div  [lindex  $B  0]  $s7] 

set  theta6a  [acos  $c6] 

if  {[greater  [abs  [minus  [sin  $theta6a]  [div  [lindex  $B  1]  $37]]]  0.01]  }  then  { 
set  theta6a  [mult  -1  $theta6a] 

> 

set  s7  [sin  $theta7b] 

set  c6  [div  [lindex  $B  0]  $s7] 

set  theta6b  [acos  $c6] 

if  {[greater  [abs  [minus  [sin  $theta6b]  [div  [lindex  $B  1]  $s7]]]  0.01]  }  then  { 
set  theta6b  [mult  -1  $theta6b] 

> 

set  theta6_eurrent  [dhtheta  6  /l.arm/dhc] 
set  theta7_current  [dhtheta  7  /l_arm/dhc] 

set  suma  [plus  [abs  [minus  $theta6a  $theta6_current]]  \ 

[abs  [minus  $theta7  $theta7_current]]] 
set  sumb  [plus  [abs  [minus  $theta6b  $theta6_current]]  \ 

[abs  [minus  $theta7  $theta7_current]]] 
if  {[greater  $suma  $sumb]>  then  { 
set  theta6  $theta6b 
set  theta7  $theta7b 
}  else  { 

set  theta6  $theta6a 

> 

> 

> 

> 

*  perp.axis  not  yet  accounted  for 

set  theta8  [dhtheta  8  /l_arm/dhc] 
set  off7  90 

set  als.angle  $theta6 

set  alf.angle  [minus  $off7  $theta7] 

set  at.angle  $theta8 

return  [list  $als_angle  $alf_angle  $at_angle] 

> 


#########»#############################*############################### 
*  vangle2 

#«##################################################»*################# 

proc  vangle2  {pi  p2}  { 

set  dot  [dot  $pl  $p2] 

set  arg  [div  $dot  [mult  [enorm  $pl]  [enorra  $p2]]] 

9 


Source  Code  Examples 


139 


t  For  the  arc  cos  procedure  the  input  needs  to  be  1  or  less 
f  If  arg  happens  to  be  just  slightly  greater  than  1  set  it  to  1 
t  so  the  arc  cos  function  sill  return  0. 

* 

if  {[greater  $arg  1]  t  [less  $arg  1.001]  I  \ 

[less  $arg  -1]  t  [greater  $arg  -1.001]}  then  { 

echo  *************  arg  sas  $arg,  set  to  [sign  $arg]  ********** 

set  arg  [sign  $arg] 

> 

set  angle  [acos  $arg] 
return  $angle 

> 

*  write.reach  -file 

proc  srite_reach_f ile  {sp  {object  glas}  {hand.angle  -10}  {f inger.tiraesteps  3}  \ 
{end.event  {return  stop}}  }  { 

set  count  [minus  [llength  $sp]  2] 

if  {$count  <  6}  then  { 

set  f inger.timesteps  $count 

} 

set  finger.count  [minus  $count  $f inger.timesteps] 

if  ![slfile  dummy  isobj]  {io  {}  dummy} 
matrix  [matrix  $object]  dummy 
topol  [lindex  $sp  $count]  dummy 

set  filename  reach.command 
set  file  [open  $filename  s] 

echo  finger.count  $f inger_count 
if  {$finger_count  ■*  0}  { 

puts  $file  "bafiogp  Jfingei'.timesteps" 

} 

for  {set  i  1}  {$i  <  [minus  $count  1]}  {incr  i}  { 
if  {$i  ==  $finger_count}  { 

puts  $file  "bafiogp  $f inger.timesteps" 

} 

puts  $file  "move_arm_to_goal  [list  [lindex  $sp  $i]]  ee  1  1  \{" 

} 

if  {$i  ==  $finger_count}  { 

puts  $file  "bafiogp  $finger_timesteps" 

} 

puts  $file  "gpp  2" 

puts  $file  "reach_object  dummy  ee  $hand_angle  0  0  \{" 
puts  $f ile  "gpp  0.4" 

puts  $file  "reach_object  glas  ee  Jhand.angle  0  0  \{" 
puts  $file  "$end_event" 

for  {set  i  1}  {$i  <=  $count}  {incr  i}  { 
puts  $file  "\}" 

} 

close  $file 

return  "file  $filename  uritten" 

} 
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Appendix  C 
User  Manual 

1  Running  the  SkillBuilder 


Start  up  3d.  Invoke  the  SkillBuilder  by  typing 
source  SkillBuilder 

at  the  3d>  prompt.  A  graphics  window  with  the  actor  standing  behind  a  high  table 
with  a  glass  on  top  will  appear  as  well  as  the  3d  Menu,  the  Command  Loop  menu, 
and  the  Views  menu.  The  actors  coordinate  system  is  located  in  the  centroid  of  the 
pelvis.  The  x-axis  points  to  the  left  side  of  the  actor,  the  y-axis  to  the  back,  and  the 
z-axis  points  upward. 

Besides  the  SkillBuilder  commands  you  can  use  any  3d  build-in  command  a  list  of 
which  can  be  found  in  a  technical  report  from  the  Computer  Graphics  and  Animation 
Group  about  “The  3d  Virtual  Environment  and  Dynamic  Simulation  System”,  written 
by  Chen  and  Zeltzer  in  August  1992.  The  3d  build-in  commands  most  often  used  in 
a  SkillBuilder  session  will  also  be  explained  in  this  manual. 

User  interaction  is  possible  through  one  of  the  menus  or,  as  long  as  the  command 
loop  is  not  running,  through  typed  input  at  the  3d>  prompt.  While  the  command 
loop  is  running  (see  section  6.1  of  this  manual),  commands  can  still  be  typed  in  the 
Do  Command  Once  text  field  of  the  Command  Loop  menu. 
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2  Changing  the  View 

2.1  Using  the  Mouse  inside  the  Graphics  Window 

In  the  3d  Menu,  click  the  LookAt  Cursor  toggle  button  right  below  the  Mouse 
Handlers  headline  to  enable  the  view  changing  by  mouse  click  in  the  graphics  win¬ 
dow. 

Depending  on  which  mouse  button  you  press,  the  following  actions  will  take  place: 

1.  Right  mouse  button:  Zooming  in  and  out. 

The  higher  you  click  in  the  graphics  window  above  the  centerline,  the  farther 
you  will  zoom  in  with  every  click.  Accordingly,  you  will  zoom  out  by  clicking 
below  the  centerline.  The  lower  you  click  inside  the  window,  the  more  you  will 
zoom  out. 

2.  Middle  mouse  button:  Toggling  the  coordinate  frame  on  and  off  and  chang¬ 
ing  the  current  object. 

The  object  the  mouse  pointer  points  at  is  made  the  current  object.  With  every 
mouse  click,  the  coordinate  frame  is  posted  and  unposted  alternately  at  the 
centroid  of  the  current  object. 

3.  Left  mouse  button:  Changing  the  view. 

If  you  click  above  the  center,  you  will  look  at  the  scene  from  more  above.  In 
contrast,  clicking  below  the  centerline  will  let  you  look  from  farther  below.  In 
the  same  way,  you  will  “walk  around”  the  current  object  to  the  right  side  by 
clicking  in  the  right  half  of  the  window,  or  to  the  left  side  by  clicking  in  the  left 
half. 

2.2  Using  the  Views  Menu 

The  text  field  at  the  bottom  of  the  Views  menu  contains  the  name  of  a  file  in  which 
views  are  saved,  or  are  to  be  saved.  The  views  file  name  can  be  changed  by  typing 
another  name  in  that  text  field. 

Clicking  the  Read  button  causes  the  views  saved  in  that  file  to  be  read  in,  i.e.  made 
available  to  be  called  ofF.  Their  names  will  be  displayed  in  as  many  fields  next  to  the 
numbered  buttons  as  there  were  views  saved  in  the  file.  To  display  one  of  the  views, 
the  corresponding  toggle  button  has  to  be  clicked.  The  view  in  question  is  now  the 
current  view.  Clicking  the  Display  button  displays  the  chosen  view. 
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To  record  a  new  view,  the  current  view  has  to  be  changed  (you  may  use  the  mouse 
or  the  keyboard).  Once  the  new  view  to  be  saved  is  found,  one  of  the  toggle  buttons 
next  to  an  empty  text  field  has  to  be  chosen  and  the  Record  button  has  to  be  clicked. 
The  word  “recorded”  will  appear  in  the  text  field  and  can  be  changed  to  name  the 
view.  To  save  the  new  view  for  use  in  the  next  SkillBuilder  session,  click  the  Write 
button  so  the  current  views  file  saves  the  new  information. 


3  Changing  the  Virtual  World 

3.1  The  Virtual  World  after  Starting  the  SkillBuilder 

The  virtual  world  of  the  actor  contains,  besides  the  actor  itself,  a  few  objects  in  the 
highest  directory.  These  are:  grid,  table,  glass,  balll,  and  ba!12.  The  grid  is  the  floor 
the  actor  stands  on.  The  table  is  placed  right  in  front  of  the  actor  and  the  glass  is 
positioned  on  the  table.  The  balls  are  not  posted,  i.e.  displayed,  but  can  be  posted 
upon  request. 

You  can  get  a  list  of  available  objects  using  the  command 
lo 


3.2  Instancing  Objects 

To  instance  an  object  use  the  3d  build-in  command 
hio  filename  object 

For  some  objects  convenience  routines  have  been  provided  that  include  (besides  in¬ 
stancing  the  object)  coloring,  scaling,  positioning,  and  hardening  of  the  object.  Pro¬ 
cedures  of  this  kind  are: 

makeglas 

make cup 

makephone 

makeroom 

makegrid 

makeceiling 

maketable 

makesoccerballs 
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3.3  Defining  the  Current  Object 

Clicking  on  the  Current  Object  button  in  the  3d  Menu  will  cause  a  pull  down 
menu  listing  all  currently  existing  objects  to  appear.  If  one  of  those  objects  is  chosen, 
it  will  appear  in  the  text  field  right  below  the  Current  Object  button.  The  current 
object  can  also  be  set  by  clicking  on  the  object  in  question  with  the  middle  mouse 
button,  or  by  typing  the  name  of  the  object  in  the  text  field. 


3.4  Displaying  Objects 

Every  object  can  be  posted,  i.e.  displayed,  or  unposted  upon  request  by  one  of  the 
3d  build-in  commands 

post  object 
unpost  object 

The  next  time  the  image  is  rendered,  the  specified  object  will  appear  on  or  disappear 
from  the  screen. 

Objects  can  also  be  easily  posted  or  unposted  by  chosing  the  option  Post  or  Unpost 
in  the  pull  down  menu  which  appears  when  clicking  on  Object  in  the  upper  left 
corner  of  the  3d  Menu.  The  object  that  will  be  posted  or  unposted  is  the  current 
object. 

3.5  Finding  the  Location  of  an  Object 

An  object’s  location  can  be  requested  by  the  3d  build-in  command 
centroid  object 

3.6  Moving  an  Object 

Objects  can  be  move  by  the  3d  build-in  commands: 

to  x  y  z  object 
ro  axis  angle  object 

or  by  using  the  Object  Edit  menu  that  pops  up  after  clicking  Object  in  the  upper 
lefthand  corner  of  the  3d  Menu. 

In  the  Object  Edit  menu  toggle  buttons  are  provided  to  choose  the  action  to  be  taken 
by  the  current  object  (whose  name  appears  below  the  Current  Object  button  in 
the  3d  Menu).  The  current  object  can  be  translated,  rotated,  scaled,  or  changed  in 
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color.  For  the  first  three  actions,  the  arrow  buttons  in  the  menu  correspond  to  the 
x,  y,  and  z  axis  in  this  order,  whereas  when  changing  the  color,  those  buttons  control 
the  r,  g,  and  b  value  in  the  rgb  code.  The  +  and  the  -  buttons  change  all  three 
values  at  the  same  time  by  the  same  amount.  Choosing  “local”  in  the  Transform 
field  makes  the  centroid  of  the  object  the  reference  frame.  A  different  reference  frame 
can  be  chosen  by  typing  its  three  coordinates  in  this  text  field  instead  of  “local”. 

To  position  an  object  on  the  upper  horizontal  plane  of  another  object,  the  procedure 

position  object  plane  nr.steps  xyplane  x  y 

can  be  called,  object  denotes  the  name  of  the  object  to  be  positioned,  whereas  plane 
denotes  the  object,  the  former  one  is  to  be  positioned  on  (e.g.  “table”).  The  param¬ 
eter  nr.step  determines  in  how  many  timesteps  the  animation  (of  the  object  moving 
from  its  current  position  towards  the  new  one)  should  be  performed.  If  “specified”  is 
chosen  for  the  xyplane  parameter  the  values  of  the  parameters  x  and  y  will  be  taken 
into  account.  The  object  will  then  be  positioned  x  inches  in  x  direction  and  y  inches 
in  y  direction  away  from  the  centroid  of  the  plane  object  at  the  maximum  z  value  of 
the  bounding  box  of  plane.  For  any  other  value  of  the  xyplane  parameter,  the  object 
will  be  placed  at  the  closest  point  on  the  plane  from  where  it  is  currently  located. 


4  Getting  Information  about  the  Figure’s  State 

4.1  Checking  the  Current  Joint  Angles 

The  current  joint  angles  of  the  9  joints  of  the  left  arm  can  be  retrieved  one  by  one  by 
the  following  commands: 

get_shoulder_fb_angle 

get_shoulder_ld_angle 

get_arm_ls_angle 

get_arm_lf_angle 

get_arm_turn_angle 

get_elbow_angle 

get_hand_turn_angle 

get  _hand_bend_angle 

get_hand_twist_angle 

or  all  at  once  (in  the  order  of  the  above  procedures)  by 
get_all_arm_angles 

The  current  joint  angles  of  the  finger  joints  can  be  retrieved  by  the  following  com- 
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mands 

get_f  inger_spread_angle  finger 
get_f  inger-bending-angle  finger  joint 
get_thumb_down_angle 
get_thumb_turn_angle 

where  the  parameter  finger  can  be  specified  as  “thumb”,  “index”,  ‘  middlefinger  , 
“ringfinger”,  or  “littlefinger” .  The  joint  should  be  set  to  either  “1”,  “2”,  or  3  .  All 
finger  angles  can  be  retrieved  at  once  by 
get_all_f  inger_angles 


4.2  Checking  the  Current  Joint  Positions 

Joint  positions  for  the  left  arm  can  be  inquired  by  the  commands 

get_shoulder_position 

get_elbow_position 

get_wrist_position 

The  end  effector  position  of  the  left  arm  which  is  located  on  the  palm  vector  will  be 
returned  by 
get_ee 

4.3  Checking  the  Current  Orientation  of  Body  Parts 

To  check  the  orientation  of  the  arm  links,  one  of  the  following  commands  has  to  be 
used: 

get_upper_arm_direct  ion 
get_forearm_direction 
get_hand_direction  option 

The  option  for  the  get_hand_direction  procedure  can  be  set  to  palm  ,  point  ,  or 
“thumb”  specifying  the  axis  of  the  hand  that  is  in  question. 


5  Local  Motor  Programs 

Local  motor  programs  are  procedures  that  cause  a  rotation  about  one  of  the  joint 
axes  of  the  figure.  They  are  not  implemented  as  skills,  so  they  should  be  invoked 
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outside  of  the  command  loop  by  typing  the  appropriate  line  at  the  3d>  prompt.  For 
the  arm  there  are 

move_shoulder  option  angle  timesteps  renopt 
lift/drop_shoulder  option  angle  timesteps  renopt 
lift_arm_sideward  option  angle  timesteps  renopt 
lift_arm_forward  option  angle  timesteps  renopt 
turn_arm  option  angle  timesteps  renopt 
bend.elbow  option  angle  timesteps  renopt 
turn-1  ower_arm  option  angle  timesteps  renopt 
bend_hand  option  angle  timesteps  renopt 
twist_hand  option  angle  timesteps  renopt 

Local  motor  programs  to  control  the  finger  and  thumb  joints  are 

spread-finger  finger  option  angle  timesteps  renopt 
bend_a_f  inger_joint  finger  joint  option  angle  timesteps  renopt 
move_thumb_outwards  option  angle  timesteps  renopt 
turn-thumb  option  angle  timesteps  renopt 
move_thumb_down  option  angle  timesteps  renopt 
bend_thumb -joint  joint  option  angle  timesteps  renopt 

The  parameters  to  a  local  motor  program  are  the  option,  which  specifies  whether 
the  appropriate  joint  should  be  moved  “by”  the  specified  angle  or  “to”  the  specified 
joint  position,  the  joint  angle  (be  it  the  absolute  or  relative  one),  the  number  of 
timesteps  in  which  the  motion  should  be  performed,  and  the  renopt  that  can  be  set 
to  “ren”  or  “noren”  depending  on  whether  the  image  shall  be  updated  on  the  screen, 
i.e.  rendered,  with  every  timestep  or  not. 

The  spread-finger  and  bend_a_f inger_joint  procedures  have  an  additional  pa¬ 
rameter,  finger ,  to  specify  the  finger  in  question,  including  the  thumb.  The  joint 
parameter  can  take  “1”  or  “2”.  The  third  bending  joint  of  a  finger  will  automatically 
be  bent  with  the  second  one  by  2/3  of  the  specified  angle. 


6  Invoking  Skills 

6.1  The  Command  Loop 

The  execution  of  a  skill  can  be  invoked  by  typing  the  skill’s  name  in  the  Do  Com¬ 
mand  Once  text  field  of  the  Command  Loop  menu  if  the  command  loop  is  already 
running.  Otherwise,  the  skill’s  name  can  be  typed  at  the  3d>  prompt  and  the  com¬ 
mand  loop  can  be  started  thereafter  by  clicking  the  Go  button  in  the  Command 
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Loop  menu.  All  skills  will  add  the  necessary  procedures  for  their  execution  to  the 
command  list.  The  Command  Loop  menu  shows  the  current  command  list  and 
has  also  buttons  to  stop  the  command  loop  and  to  step  through  the  list.  Access 
procedures  that  are  able  to  add,  insert,  or  remove  commands  from  the  command  list 

are 

add_c  command 

insert_c  command  nr.nexL command 
rem_c  command 


6.2  Timing  and  End  Events 

Every  skill  has  a  parameter  end-event  that  is  set  to  “{}”  as  a  default.  The  end 
event  specifies  what  should  happen  when  the  execution  of  the  skill  is  completed.  For 

example 

wave_hand  { 
global  org 

move_arm_to_conf  ig  $org  0  0  {} 

} 

will  link  the  two  skills  (waveJhand  and  move_arm_to_conf  ig)  sequentially  which  causes 
the  arm  to  return  into  its  original  position  after  the  hand  waving  skill  stops.  In  or¬ 
der  to  keep  the  last  state  of  a  skill  in  the  command  list,  end-event  has  to  be  set  to 

“return” . 

Inverse  kinematics  skills  all  contain  two  parameters  to  control  the  velocity  of  the 
skill:  time-opt  and  timesteps.  If  the  time.opt  parameter  is  set  to  “1”,  the  target 
number  of  timesteps  is  set  to  the  number  specified  by  the  timesteps  parameter.  If 
the  time-opt  parameter  is  set  to  “0”,  the  number  of  timesteps  for  the  execution  of 
the  skill  is  automatically  calculated  and  the  timesteps  parameter  has  no  effect.  The 
timesteps  calculation  for  the  inverse  kinematics  skills  is  based  on  the  global  parameters 
dist_per_timestep.  The  configuration  skills  (see  section  6.3),  as  well  as  the  skills  to 
orient  a  body  part  (see  section  6.4),  rely  on  the  global  parameter  angle_per_timestep 
which  can  be  set  by  the  procedure 

aps  angle 

If  no  angle  is  specified,  the  procedure  returns  the  current  value  of  angle_per_timestep 
(in  degrees).  Similarly,  the  dist_per_timestep  can  be  changed  using  the  procedure 

dps  distance 

and  the  value  of  the  current  dist_per_timestep  (in  inches)  can  be  retrieved  by  calling 
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the  dps  procedure  without  an  argument. 


6.3  Configuration  Skills 

Bringing  a  limb  into  a  specific  configuration  can  be  achieved  by  one  of  the  the  com¬ 
mands 

move_arm_to_conf  ig  config  time-opt  timesteps  end-event 
bringjf inger_in_conf  ig  finger  config  timesteps  end-event 
bring_thumb_in_conf  ig  config  timesteps  end-event 

The  move_arm_to_conf  ig  skill  expects  a  9  dimensional  vector  as  input  for  its  config 
parameter  specifying  the  following  target  angles:  shoulder-fb  angle,  shoulder-ld  angle, 
arm-ls  angle,  arm-lf  angle,  arm-turn  angle,  elbow  angle,  hand-turn  angle,  hand-bend 
angle,  and  hand-twist  angle. 

For  the  bringjf  inger_in_conf  ig  skill,  the  parameter  config  has  to  be  a  three  dimen¬ 
sional  vector  specifying  target  angles  for  the  following  angles  of  the  finger:  spread 
angle,  bendl  angle,  and  bend2  angle. 

The  configuration  config  for  the  thumb  has  to  be  a  5  dimensional  vector  specifying  the 
spread  angle,  down-angle,  turn-angle,  bendl  angle,  and  bend2  angle  for  the  thumb. 

The  following  skills  will  change  the  hand  into  a  specific  posture: 

bring_alljf ingers_in_rest_conf  ig  timesteps  end-event 
bring_alljf ingers_in_point_conf  ig  timesteps  end-event 
bring_alljfingers_in_open_grasp_conf  ig  timesteps  end-event 


6.4  Body  Part  Orientation  Skills 

In  order  to  change  the  orientation  of  a  body  part,  one  of  the  following  skills  can  be 
used  for  the  upper  arm,  the  forearm  or  the  hand: 

direct_upper_arm  main-axis  perp.axis  time-opt  timesteps  end-event 
direct  jforearm  main-axis  time-opt  timesteps  end-event 

direct_hand_palm/point_axes  palm-axis  point-axis  time-opt  timesteps  end-event 
direct_hand_palm_towards  object  time-opt  timesteps  end-event 
align_hand_thumb_axis_with_obj  ect  object  hand-bend-angle  time.opt  timesteps 

end-event 

The  main_axis  in  the  direct_upper_arm  and  the  direct_f orearm  skills  has  to  be 
a  3  dimensional  vector  specifying  a  target  orientation  for  the  axis  of  the  upper  arm 
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or  the  forearm  respectively.  It  does  not  necessarily  have  to  be  a  unit  vector.  The 
perp_axis  parameter  in  the  direct_upper_arm  skill  is  usually  set  to  “no”,  meaning 
that  no  specific  perpendicular  axis  has  to  be  reached.  With  perpendicular  axis  is  an 
axis  denoted  that  is  perpendicular  to  the  main  axis  and  forms  together  with  the  main 
axis  the  plane  in  which  the  forearm  is  located. 

The  direct _hand_palm/point_axes  skill  has  an  option  to  specify  both  a  target  palm 
axis  and  a  target  point  axis,  each  consisting  of  three  dimensional  vectors.  In  case 
both  axes  are  given,  all  three  hand  angles  will  be  influenced.  In  case  there  is  no  point 
axis  specified  (by  setting  the  point_axis  parameter  to  “no”),  the  current  twist  angle 
of  the  hand  will  remain  the  same,  since  it  does  not  influence  the  palm  axis. 

For  orienting  the  palm  axis  towards  an  object  the  skill  direct_hand_palm_towards 
is  provided.  It  is  very  similar  to  the  previous  one  when  no  point  axis  is  specified: 
no  hand  twisting  will  take  place.  This  skill  can  e.g.  be  used  in  order  to  “look  at  a 
picture”  if  the  picture  is  placed  in  the  hand  parallel  to  the  palm,  the  palm  is  oriented 
towards  the  head  and  the  head/eyes  are  oriented  towards  the  picture. 

Finally,  the  skill  align Jiand_thumb_axis_with_object  aligns  the  thumb  axis  with 
the  longitudinal  axis  of  an  object  by  changing  the  hand-twist  and  hand-turn  angles. 
The  hand-bend  angle  does  not  play  a  role  when  aligning  the  thumb  axis  and  can 
therefore  freely  be  chosen. 

6.5  Reaching  and  Grasping  Skills 

There  are  the  following  reaching  skills: 

move_arm_to_goal  goaLpoint  gp-opt  time-opt  timesteps  end-event 
move_arm_to_object  object  gp-opt  time-opt  timesteps  end-event 
reach-object  object  gp.opt  hand-bend-angle  time.opt  timesteps  end-event 
reach_object_along_path  object  hand-bend-angle  end-event 

Positioning  of  the  end  effector  at  a  specified  goal-point  takes  place  when  calling  the 
procedure  move_arm_to_goal  with  gp-opt  set  to  “ee”.  If  the  wrist  is  to  be  positioned 
at  the  goal  point,  the  parameter  gp-opt  should  be  set  to  “wrist”.  The  same  applies  for 
the  skill  move_arm_to_object,  but  in  this  case,  the  actor  will  reach  for  the  centroid 
of  the  object. 

The  skill  reach-object  incorporates  an  alignment  of  the  hand  along  the  longitudinal 
axis  of  the  given  object.  The  final  hand-bend-angle  can  freely  be  chosen  and  is  set  to 
“-10”. 

In  none  of  the  skills,  move_arm_to_goal,  move_arm_to_object,  or  reach-object,  will 
possible  collisions  be  avoided. 
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A  collision  free  path  is  generated  by  the  reach_object_along_path  skill  and  the  end 
effector  of  the  actor  is  guided  along  that  path.  The  skill  includes  forming  an  open 
grasp  position  with  the  hand  (shortly  before  the  object  is  reached)  and  wrapping  the 
fingers  around  the  object,  once  it  is  reached. 

The  grasping  skill  can  also  be  invoked  separately  by  the  skill 

grasp_object  object  speed  end-event 

where  the  speed  should  be  a  factor  between  0.1  and  5.  A  fist  can  be  made  by 
makejfist  speed  end-event 


6.6  Oscillating  Skills 

An  oscillating  skill  is 

wave_hand  end-event 

The  hand  waving  skill  can  be  stopped  by  typing  stop.waving  in  the  Do  Command 
once  textfield. 

6.7  Head  and  Eye  Control  Skills 

There  are  three  skills  to  control  the  head  and  eye  motions: 

head-track  object  end-event 
head-straight  time-opt  timesteps  end-event 
head_move  object  time-opt  timesteps  end-event 

The  head_move  skill  will  cause  the  head  and  eyes  to  move  the  focus  towards  the 
specified  object  in  the  given  number  of  timesteps  and  evaluate  its  end-event  upon 
completion  of  the  skill.  The  skill  will  be  able  to  adapt  to  a  moving  object. 

Keeping  the  focus  of  the  head  and  eyes  on  a  moving  object  can  be  done  by  invoking 
the  head-track  skill.  It  is  useful  to  define  head-track  as  end  event  of  head_move, 
in  order  to  slowly  move  the  focus  towards  an  object  before  tracking  it.  head-track 
is  a  continuous  skill  which  means  it  will  only  stop  executing  when  it  is  explicitly 
stopped  by  calling  the  procedure  stop_head_track.  Remember  that  while  the  com¬ 
mand  loop  is  running,  interactions  are  only  possible  through  the  menus.  This  means 
stop_head_track  has  to  be  typed  in  the  Do  Command  Once  text  field. 

The  skill  head-straight  brings  the  actor’s  focus  back  to  a  point  directly  in  front  of 
him  at  the  level  of  the  eyes  much  like  the  headjnove  skill  brings  the  focus  towards 
an  object. 
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7  Exiting  the  SkillBuilder 

Use  exit  instead  of  quit  when  exiting  the  program.  Otherwise,  the  manipulators  for 
the  head  controller  and/or  the  data  glove  might  keep  running. 
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